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ABSTRACT 


The formation problem of distributed mobile robots was studied in the literature 
for idealized robots. Idealized robots are able to instantaneously move in any direction, 
and are equipped with perfect range sensors. In this study, the formation problem of 
distributed mobile robots that are subject to physical constraints is addressed. Mobile 
robots considered in this study have physical dimensions and their motions are governed 
by physical laws. They are equipped with sonar and infrared range sensors. The 
formation of lines and circles by using the potential field method is investigated in detail. 
It is demonstrated that line and circle algorithms developed for idealized robots do not 
work well for physical robots. New line and circle algorithms, with consideration of 
physical robots and sensors, are presented and validated through extensive simulations. 
Movement in formation of a small group of physical mobile robots is also studied. An 
algorithm is developed using the potential field method that makes robots move through a 
workspace filled with many obstacles while maintaining the formation. 
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I. INTRODUCTION 


A. GENERAL 

Given a group of mobile robots (say, 20 robots) randomly placed on a laboratory 
floor, how would one control them to form a geometric pattern such as a circle without 
using a centralized coordinator? This is the formation problem of distributed mobile 
robots studied in References 1-5. Distributed robots make motion plans based on a given 
task goal of the group and the perceived information about their environment from 
onboard sensors without the aid of a centralized coordinator. 

The formation problem of distributed mobile robots has been studied for idealized 
mobile robots [Ref. 1, 2, 4] — robots that are represented by a point, able to move in any 
direction, and equipped with range sensors that can determine the position of all other 
robots. Since a robot is a point, two or more robots may occupy the same location. Each 
robot has its own coordinate system and there is no common, global coordinate system. 
Furthermore, these robots do not communicate with each other. Under these 
assumptions, Suzuki et. al have developed a number of distributed formation algorithms. 
In particular, they developed algorithms for multiple distributed mobile robots to form 
circles, simple polygons and line segments; to uniformly distribute robots within a circle 
or a convex polygon; and divide them into groups [Ref. 1,2,4, 5], 

In the previous study [Ref. 1, 2, 4], even though the number of robots 
participating in a given task is assumed to be unknown, the perfect sensor assumption 
makes it possible for each robot to “see” the location of all other robots, and hence to 
determine the number of robots. Perfect sensors are not occluded by the presence of other 
robots. One of the biggest challenges in implementing existing formation algorithms is 
the inability to sense the location (or even just the presence) of all other robots by using 
sonar or infrared sensors. Each robot may see a different number of robots at each instant 
of time. 
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Line and circle formation, or formation of any geometric patterns in general, is 
only one of many issues of distributed mobile robots [Ref. 6]. Representative work 
addressing other issues of distributed mobile robots includes cellular robotics systems 
[Ref. 7, 8, 9], and dynamically reconfigurable robotics systems [Ref. 10]. These systems 
can change their overall shape depending on the task and the environment by 
autonomously detaching and combining cells. 

B. PROBLEM STATEMENT 

Based on earlier work, this thesis studies the formation problem of distributed 
“physical” mobile robots. The mobile robots considered in this study have physical 
dimensions (hence two robots cannot occupy the same spot), and their motions obey 
physical laws (hence wheeled mobile robots must satisfy nonholonomic constraints). 
Furthermore, robots are assumed to be equipped with range sensors having realistic 
physical properties. The robot simulator from Nomadic Technologies, Inc. is used. 
Robots in the simulator realistically simulate the motion behavior and sensor systems of 
the Nomad 200 mobile robots [Ref. 21, 22], The Nomad robot has a synchronous drive 
mechanism which enables it to translate, steer, and rotate (its turret) independently. The 
robot is nonholonomically constrained, thus not able to instantaneously move in the 
lateral direction. The robot's sensor systems include tactile (bumper) sensors, infrared 
sensors, ultrasonic sensors, and laser sensors. All but the laser sensors are used in the 
simulation of this study. 

This thesis also studies the development of behaviors for formation maintenance 
in a small group of physical mobile robots. In addition to maintaining their position in 
formation, robots must also move to a specified goal location while avoiding collisions 
with obstacles and other robots. Most work done in this area focused on the dynamics 
and stability of multi-robot formations. Ref. 11 developed a strategy for robot formations 
where individual robots are given specific locations relative to a leader or neighbor to 
maintain their positions. Ref. 12 demonstrated formation generation by distributed 
control. Both studies centered on the analysis of group dynamics and stability and did not 
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provide obstacle avoidance. Ref. 13 integrated motor schemas, for relative positional 
maintenance with existing navigational behaviors to help robots complete navigational 
tasks while in formation. The formation behaviors were implemented as motor schemas, 
a type of reactive navigational strategy [Ref 14]. Each robot is assigned a unique 
identification number (ID), and a robot's designated position in a given formation depends 
upon its ID. 

C. IMPLEMENTATION OF COLLISION AVOIDANCE 

Different schemes for collision avoidance were examined in References 4, 12, 14, 
15, 16, 17, 18, 19. The method proposed in Ref. 4 is discussed in the following chapter. 
The strategy proposed in Ref. 16 is that if a robot detects another robot on its way, it stops 
and waits for some fixed period of time. If a robot is still present, the robot turns left and 
proceeds forward. The method proposed in Ref. 12 adds an initial step to the algorithms 
from References 1 and 5 to avoid collisions. Motor schemas [Ref. 14] is another method 
for navigation and collision avoidance. 

Motion control and collision avoidance in this study are achieved by 
implementing a potential field algorithm [Ref. 20, 21]. To each robot of concern, the 
presence of other robots generates a repulsive force which keeps them apart, and the goal 
position produces an attractive force. Because the workspace is assumed to be obstacle- 
free, the shape of robots is circular, and the goal position changes as other robots move, 
the local minimum problem of the potential field method is rarely encountered in the 
simulations. 

D. OUTLINE OF THE THESIS 

Chapter II provides background information including the Nomad 200 mobile 
robot and the simulator as well as a brief description of the potential field algorithm. 
Chapter HI discusses the implementation of the existing line algorithm with physical 
robots and presents a modified version of the existing algorithm which works well for 
physical robots. Chapter IV studies the existing circle algorithm and simulation results 
when it is implemented with physical robots. A modified algorithm is introduced which 
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is directed towards forming a better approximation of a circle. Chapter V discusses an 
algorithm based on "leader-follower" technique to help a small group of robots move in 
formation while avoiding collisions. Chapter VI presents the conclusion and 
recommendations for follow-on studies. The source codes for the modified line 
algorithm, modified circle algorithm and the moving in formation algorithm are presented 
in Appendix A, B, and C respectively. 
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II. BACKGROUND INFORMATION 


This chapter provides background information so that the reader has an 
understanding of the NOMAD 200 mobile robot, as well as the potential field method 
which is implemented to realize the motion of robots. 

A. NOMAD 200 MOBILE ROBOT 

The Nomad 200 is an integrated mobile robot system with four sensory modules 
including tactile, infrared, ultrasonic, and laser systems [Ref. 22], The Nomad 200 has 
on-board computers for sensor and motor control and for host computer communication. 
The mobile base keeps track of its position and orientation through dead-reckoning. The 
Nomad 200 includes a software package for the host computer with a graphic interface 
and a simulator. 

1. Mechanical System 

The Nomad 200 mobile base is a three servo, three-wheel synchronous drive non- 
holonomic system with zero gyro-radius. The three wheels translate together (controlled 
by one motor) and rotate together (controlled by a second motor). A third motor controls 
the angular position of the turret. The robot can only translate along the forward and 
backward directions along which the three wheels are aligned (this is referred to as non- 
holonomic constraint, similar to that of a car). The robot has a zero gyro-radius, i.e. the 
robot can rotate around its center. 

The Nomad 200 has a maximum translational speed of 20 inches per second and a 
maximum rotational speed of 60° per second. It has a diameter of 18 inches and a height 
of 35 inches. 

2. Sensor Systems 

The robot's sensor systems include tactile (bumper) sensors, infrared sensors, 
ultrasonic sensors, and laser sensors. All but the laser sensors are used in the simulation 
of this study. The tactile system which consists of two bumper rings is used to detect 
contact with any object. 
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The Nomad 200 has a 16 channel reflective intensity based infrared ranging 
system that provides 360 degrees coverage. Each of the 16 sensors is composed of two 
LED emitters and a photodiode detector enclosed in a delrin housing. The range to the 
object(s) is determined by the intensity of the light from the emitter reflected back to the 
detectors from an object. The infrared sensors are quite accurate within 35 inches, but not 
reliable beyond 35 inches. 

The Nomad 200 also has a 16 channel sonar ranging system which can give range 
information from 17 inches to 255 inches with 1 percent accuracy over the entire range. 
The sonar system is a time of flight ranging sensor based upon the return time of an 
acoustic signal. The sensors are standard Polaroid transducers with a beam width of 25°. 
The circumference of the robot is covered by 16 sensors. 

Although the user manuals [Ref. 21, 22] for the Nomad 200 robot state that the 
maximum sonar range is 255 inches, it is determined by two parameters (half-cone and 
overlap) which are stored in the robot.setup file. The sonar sensors have a non-zero 
beam-width, i.e., they can detect an object as long as it overlaps with this cone (and is 
within detectable distance). The half beam-width for sonar is specified by half-cone in 
the setup file. If this variable is set to 0, the sensor can detect an object only if it 
intersects with the ray drawn from the sensor along the direction where the sensor is 
pointing. When this variable is set to some positive values, the sensors can detect objects 
that fall within the cone specified by this variable assuming there is sufficient overlap 
between the object and the sonar cone (as specified by overlap in the setup file). Briefly, 
half-cone sets half the angular range of the main lobe of the sonar while overlap sets the 
minimal apparent size of a surface to be detected when using the conical model. 

The sonar half-cones have to overlap in order to cover 360 degrees around the 
robot. If they overlap quite a distance away from the robot, then the triangular area that 
stays between two consecutive sonars and the intersection point of sonar beams cannot be 
covered at all. On the other hand, anything in the overlapped zone is seen by both sonars. 
This means that the position of that object cannot be calculated accurately. Worst of all, 
the sonars that see the same object which is in their overlapped zone, cannot sense 
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another object even if it is on the line of sight, unless it is closer than the one in 
overlapped zone. So, letting the sonar beams overlap very close to the robot creates a 
huge overlapped zone, which in turn affects the sight of the sonars negatively. The 
maximum sonar range in the area that is exactly in the middle of two consecutive sonars, 
is shorter than the one in line of sight area. The values of half-cone and overlap have to 
be set in such a way to best balance these factors. Table 1 illustrates the results of the 
simulations with different values of half-cone and overlap, in order to detect an object of 
a size of the Nomad 200 robot. 


Table 1. Simulation Results to Determine the Values of 
Half-cone and Overlap. 


Half-cone 1 

Overlap 2 

A 3 

B 4 

C 5 

125 

0.05 

51 

53 

62 

125 

0.07 

208 

210 

212 

125 

0.10 

128 

144 

145 

125 

0.15 

74 

75 

85 

105 

0.07 

145 

154 

236 

105 

0.08 

131 

139 

206 

105 

0.09 

119 

130 

180 

105 

0.10 

111 

115 

162 

110 

0.08 

159 

175 

194 

110 

0.09 

143 

152 

174 

115 

0.07 

185 

187 

235 

115 

0.08 

166 

168 

188 

115 

0.10 

129 

130 

146 


1 Half-cone (in tenths of degrees) 

2 Overlap (expressed as a ratio -- 0.1=10% -- to the angular range of the 
main lobe); 

3 A: The range that two consecutive sonars first start to sense the same robot 
in the overlapped zone (in inches) 

4 B: The maximum range in the overlapped zone to see a robot (in inches) 

5 C: The maximum line of sight range to see a robot (in inches) 


The first set of values in Table 1 represent the default values which are set mainly 
for the detection of large objects such as a wall. The best values for half-cone and 
overlap to detect an object of the size of the Nomad 200 robot was determined to be 10.5 ° 
and 0,8 percent respectively, and used throughout all simulations. 
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3. 


Robot Simulator 


The Nomadic Host Software Development Environment is a full featured object 
based mobile robot software development package for the Nomad 200 mobile robot [Ref. 
23]. It consists of two parts: the server and the client. The server performs four 
functions: Host <■—* Robot Interface which allows complete control of the robot from a 
host computer, Robot Simulator which simulates the Nomad 200 robot (including its 
basic motions, translation, steering and rotation, and its basic sensor systems: tactile, 
infrared, sonar and laser), Graphic User Interface which provides graphic display for 
various sensory information and convenient interface with the robot and the robot 


simulator, and Client <■ ■> Server Language User Interface which allows users' C or Lisp 
program (acts as a client process) to access the server. The client part provides the link 
(in terms of a set of interface functions) between the application program and the server. 

The graphical user interface consists of several windows that give information and 
allow control of a robot. First, there is the world (map) window which gives an overall 
view of the environment (real or simulated) that the robots are in as well as the positions 
of each robot relative to the environment and each other. Also there is the robot window, 
(one copy for each robot), which contains information about each individual robot, such 
as current command executed, position, orientation, and sensor data history. Attached to 
each robot window are two windows that give more detailed information about the 


current sensor readings, including ray and half-cone data. Each time any of the functions 
that return sensor data is called, the sensory data returned as well as the current positions 
of robots are displayed graphically on these windows. The users are allowed to draw 
maps in the world window to simulate the environment. The figures that show the 
simulation results in the following chapters are the snapshots of the world window taken 
at different time instants during a simulation. 


In order to run the simulator, the executable server program ( Nserver ), the setup 
files for the world ( world.setup ) and for each robot (robot.setup), as well as the license 
file must be in the same directory. To start the server, one simply executes the Nserver. 
Individual setup files can be specified as command line parameters. If the setup files are 
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not specified, the server will automatically look for world.setup and robot.setup. It is 
necessary to have a separate setup file for each robot to be created. The name of each 
robot setup file must be specified in the world.setup file. The best way to discriminate 
the robots is to set a different color for each robot in its own robot.setup file. 

The application program for each robot should mn simultaneously as a separate 
process, by taking advantage of multitasking capabilities of UNIX operating system. 
This makes debugging very easy and provides the possibility to test each behavior 
independently as well as to add or remove some robots during the run. 

B. POTENTIAL FIELD METHOD 

A robot in potential field method is treated as a point represented in configuration 
space as a particle under the influence of an artificial potential field U whose local 
variations reflect the “structure” of the free space. The potential function can be defined 
over free space as the sum of an attractive potential pulling the robot toward the goal 
configuration and a repulsive potential pushing the robot away from the obstacles [Ref. 
20]. Motion planning is performed in an iterative fashion. At each iteration, the artificial 
force induced by the potential function at the current configuration is regarded as the most 
appropriate direction of motion, and path planning proceeds along this direction by some 
increment. 

The general idea is that the robot is attracted toward its goal configuration while 
being repulsed by the obstacles. In this section, this idea is illustrated with the definition 
of one possible potential function, in the case where robot moves freely in W=R N , with 
N=2 or 3, i.e. C=R N . (W denotes Robot's workspace, R is the set of real numbers, C 
denotes the configuration space of a robot. An element of C is denoted by q.) A more 
detailed discussion can be found in [Ref. 20]. 
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The field of artificial forces F(q) in C is produced by a differentiable potential 
function: 

U:C free 

with: F(q) = -Vu{q) (1) 

where Vu(q) denotes the gradient vector of U at q. In C=R n (N = 2 or 3), we can 
write q=(x,y) or (x, y, z), and: 


In order to make the robot attracted toward its goal configuration, while being 
repulsed from the obstacles, U is constructed as the sum of two elementary potential 
functions: 

U i < l) = U aM + U rep {q) , (3) 

where U aU is the attractive potential associated with the goal configuration q goa i and U rep is 
the repulsive potential associated with the C-obstacle region. U att is independent of the C- 
obstacle region while U rep is independent of the goal configuration. 

With these conventions, F is the sum of two vectors: 

Fat, = ~VU att and F rep = -VU rep , ( 4 ) 

which are called the attractive and the repulsive forces, respectively. 

1. Attractive Potential 

The attractive potential field U au can simply be defined as a parabolic-well, i.e.: 

u au{q) = \&l oa ,{q ), (5) 

where ^ is a positive scaling factor and p goa ,(q) denotes the Euclidean distance \\q - q goa ,||. 
The function U M is positive or null, and attains its minimum at q goah where U M {q g J) = 0. 




10 



The function p goa , is differentiable everywhere in C. At every configuration q, the 
attractive force F att deriving from U a „ is: 

4(?)=-vc rJa) 

~ ^Pgoal {.P Pgoal (.P ) ( 6 ) 

~ ^P Pgoal ) • 

The parabolic well demonstrates good stabilizing characteristics when used for 
on-line collision avoidance [Ref. 10]. This is because it generates a force F att that 
converges linearly toward 0 when the robot's configuration gets closer to the goal 
configuration. On the other hand, F atl increases with the distance to the goal 
configuration and finally tends toward infinity when p goal (q) . Alternatively, U a „ 

can be defined as a conic-well, i.e.: 

V.,(q) = ^,(q) ■ ( 7 ) 

Then, the attractive force is: 

Fa t tW) = -&P g oalW) 


e (ff Pgoal) 
P~ Pgoal 


( 8 ) 


The amplitude of F aU (q) is constant over C, except at q goah where U a „ is singular. 
Since the amplitude of the force does not tend toward 0 when q —> q goah the conic-well 
potential does not have the stabilizing characteristics of the parabolic-well function. 

The advantages of both the parabolic and the conic-wells can be combined by 
defining the attractive potential as a parabolic-well within a distance “d” from the goal 
configuration and a conic-well beyond that distance. In this case a discontinuity problem 
is encountered as plotted in Figure 1(a) when it is implemented by using the above conic- 
well definition. The attractive force must be made continuous at the transition point, 
which can be achieved simply by multiplying the conic-well attractive potential by “d”. 
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Then, the resulting attractive force can be defined as follows, which is plotted in Figure 

1(b). 


F at M) = 


if | q-q g oai 

~ Qgool ) if I# ~ ‘l goal 

9 goa/ 


<d 


>d 


( 9 ) 


2. Repulsive Potential 

The main idea is to create a potential barrier around the C-obstacle region that 
cannot be traversed by the robot's configuration. In addition, the repulsive potential 
should not affect the motion of the robot when it is sufficiently far away from the C- 
obstacles. These constraints can be achieved by defining the repulsive potential function 
as follows: 




1 


V 


p(q) Po 


if p{q)<p 0 


0 


if p(q) 


>Po 


( 10 ) 


where 7 is a positive scaling factor, p(q) denotes the distance from q to the C-obstacle 
region CB, i.e.: 


P(q)= min \\q-q'\\ , ( 11 ) 

q’tCB 

and p 0 is a positive constant called the distance of influence (or cut-off distance) of the C- 
obstacle. The function U rep is positive or null, tends to infinity as q gets closer to the C- 
obstacle region, and is null when the distance of the robot's configuration to the C- 
obstacle is greater than p 0 . 
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(a) 



Figure 1. Plot of an attractive force that is defined by combining the attractive 
potential as a parabolic-well within a distance “d” from the goal configuration and a 
conic-well beyond that distance. Figure 1(a) illustrates the discontinuity problem, 
and (b) illustrates the provided continuity at the transition point. 
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If CB is a convex region with a piece wise differentiable boundary, p is 
differentiable everywhere in C free . Then the artificial repulsive force deriving from U rep is 
defined as follows: 


F rep {q ) = ~VU rep (q) 


V 


1 1 


PW Po)p 2 (q) 


-Vp(q) if p{q)<p 0 


if p(q)>p Q 


( 12 ) 


Let q c be the unique configuration in CB that is closest to q, i.e. that achieves 

11*1 ' qdl = P(q)- The gradient V p( q) is a unit vector pointing away from CB and 
supported by the line passing through q c and q. 

If we retract the (unrealistic) assumption that CB is convex, p(q) remains 
differentiable everywhere in Cfr ee , except at those configurations q for which there exist 
several q c e CB verifying ||q - q c || = p(q). These configurations form a set of measure 
zero in C which is in general locally (N - l)-dimensional. The force field f is defined 

rep 

on both sides of this set, but with differently oriented vector values. This may result in 
producing paths that oscillate between the two sides of the set. At those configurations q 
for which there exists only one q c e CB verifying ||q - q c || = p(q) at a time, there may be 
big jumps between the angular directions of consecutive q c configurations during the 
motion of robots. This may cause serious oscillatory behaviors which in turn demolishes 
the robustness in motion. 

One way to eliminate this difficulty is to find out the exact profile of the C- 
obstacle region CB, and then make the motion planning. This requires too much 
computation and it is not possible to obtain the exact profile with available sensors. 
Another way is to decompose CB into convex components CB,, i = 1,...r, and to associate 
a repulsive potential Ucbi with each component. This is easy to implement in Nomad 200 
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mobile robot by using 16 sensors that have 22.5° between two consecutive sensors, which 
automatically decomposes CB into 16 components. Each obstacle detected by any sensor 
(whether it is a separate obstacle or a part of a big obstacle) produces a repulsive force. 
The resulting repulsive force is the sum of repulsive potential fields created by each 
individual sensor contact. Thus, the resulting artificial repulsive force is: 

15 

, ( 13 ) 

;'=0 

where i represents the sensor number. In the Nomad 200 robot, there is one infrared and 
one sonar sensor that can scan the same direction. Infrared sensor information is 
considered if the returned value is within maximum infrared sensor range, and the sonar 
information is considered otherwise. 
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IU. FORMATION OF A LINE SEGMENT 


This chapter discusses the results and associated problems of the existing 
algorithm when it is implemented with physical robots. Then a modified version of the 
existing algorithm which works well for physical robots is presented. 

A. EXISTING LINE ALGORITHM 

The following is the original line algorithm proposed in Ref. 5. It is assumed that 
each robot repeatedly becomes active and inactive (sleep mode) at unpredictable time 
instants. Each time a robot becomes active, it does the following: 

• Step 1. Determines the furthest robot i?/and the closest robot R c . 

• Step 2. Calculates the distance d from its current position to the point p 

that is the foot of the perpendicular drop from itself to the line passing through 
R c and Rj. 

• Step 3. Moves min {d,v} towards point p, where v is the maximum distance 

the robot can move at a time. 

Assuming that each robot is a dimensionless point, the algorithm enables all robots to 
form a line segment [Ref. 5]. In a revised version of the algorithm, the physical 
dimension of the robots was considered, and a simple collision avoidance strategy was 
implemented [Ref. 4]. The strategy works as follows: 

• If a robot detects another robot within a certain distance in the direction of its 
move, it then swerves to the left minimally, provided that it successfully finds 
a direction that is clear of any robots. 

• If no such left swerve is found possible, the robot decides not to move until 
either its path becomes clear or a suitable left swerve becomes possible. 

First, this algorithm was simulated without any collision avoidance scheme, and 
an unacceptable number of collisions was experienced which prevented robots from 
forming a line. Then the algorithm was implemented with the simple left-swerve collision 
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strategy and 20 simulations was run, each time with a random initial distribution of the 
robots. A number of problems with the implementation of the algorithm were observed. 

A problem occurs when four or more robots are very close to each other and try to 
avoid collisions. The robot with whom each one is trying to avoid collision changes 
frequently (so do R c and R/ for each robot), which in turn changes the goal configuration 
of each one. In this case the robots jam each other. Another frequently encountered 
problem is illustrated in Figure 2. For the convenience of discussion, let us name the 
robots Ri to R 6 from the upper-right comer to the lower-left comer in Figure 2(a), 
respectively. Figure 2(a) is a typical distribution where robots get closer to form a line 
segment. At that moment, i ?3 swerves to its left to avoid R4. R.\ moves downward to its 
goal location, which is the perpendicular drop to the line passing through the closest robot 
i?3 and furthest robot R5. At the same time, the other robots go through a similar process. 
Figure 2(b) illustrates the distribution of the robots a few iterations later. 

As the simulation continues, robots reconvene into a distribution similar to the 
one shown in Figure 2(a). Robots repeat the motion sequences and fail to form a line 
segment. 

It is observed that robots form a line segment only when the initial distribution is 
very close to a line segment and little or no collision avoidance is required. Finally, it is 
noted that a uniform distribution of robots along the line segment cannot be accomplished 
using this method. 

Aiming to overcome the collision avoidance problem encountered in the above 
implementation, the left-swerve strategy was replaced with the potential field method. It 
turns out that the potential field method does not help at all. The robots show various 
group behaviors other than forming a line segment. The result of a simulation is 
discussed below. 

Figure 3(a) shows the initial, random distribution of six robots while Figure 3(b) 
to 3(e) illustrate their progressive movements. As soon as the simulation begins, robots 
start getting closer to each other as a natural result of the algorithm. A problem occurs 
when point p is within the physical dimensions or repulsive force range of a robot or 
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between two robots that do not have enough room in-between for another robot. 
Unfortunately this happens in most simulations, unless the initial distribution is very 
close to a line segment. 
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Figure 2. Implementation of the existing algorithm with the left-swerve collision 
avoidance strategy. 
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(e) (f) 


Figure 3. Selected images of the existing line algorithm simulation using the 
potential field method from an initial distribution to a final stage: (a) the initial 
distribution, (b)-(e) intermediate steps, and (f) the final distribution of the robots 
trying to form a line segment. The two robots on the lower-left side are 
approximately where they should be while the remaining ones are in deadlock. 
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Eventually robots approach a deadlock configuration as shown in Figure 3(f). In 
this configuration, the attractive force generated by goal point p is negated by the 
repulsive force generated by surrounding robots. Consider the robot at the top in Figure 
3(f), for instance. Its furthest robot Rj is the upper one in the two-robot group (it cannot 
see the lower one in the group), and its closest robot R c is the closest one in the usual 
sense. Goal point p in this case corresponds to a point within the closest robot's repulsive 
force range. This is also true for all other robots in the four-robot group. 

Another line algorithm is proposed in Ref. 1 and Ref. 4. The user selects two 
robots that will be the endpoints of the line segment and does the following, possibly 
concurrently: 

• Move the two robots manually to their desired final positions. 

• Let all other robots execute algorithm FILLPOLYGON 

where FILLPOLYGON is a technique proposed in Ref. 4 to distribute robots nearly 
uniformly within a convex polygon, starting from an arbitrary initial distribution except 
anomalies. 

This algorithm is not implemented in this study as the first task is clearly not 
distributed, and it does not coincide with the autonomous characteristics of distributed 
mobile robots. 

B. MODIFIED LINE ALGORITHM 

Although the existing algorithm does not work well for physical robots to form a 
line segment, its main idea is still valid. As discussed above, a problem occurs if the goal 
point p on the line passing through the closest (R c ) and furthest (Rj) robots is occupied by 
another robot, or if there is not enough room for another robot at the goal point. To 
circumvent the problem, the algorithm was modified so that the goal point p is still 
chosen to be on the same line, but at a location where there is room for another robot. 
Since each robot executes the same program, the discussion below is concerned with a 
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robot called R for convenience, which can be any one of the robots. At each iteration, 
robot R does the following: 

• Step 1. Determines the closest robot R c and furthest robot R f based on its 
sensor readings. 

• Step 2. Determines if point p, the foot of the perpendicular drop from its 
current position to the line l passing through R c and Rf, is between R c and Rf. 

• Step 3. If yes, and if there is enough room for robot R to fit in-between R c 

and Rf it proceeds towards the mid-point, which is denoted by p m . 

• Step 4. If p is not between R c and Rf or if there is not enough room 
between R c and Rf it proceeds towards point pd on the line / which is d 
distance away from R c in the opposite direction of Rf where d is the minimum 
distance that would prevent any repulsive force being applied to either robot. 

Figure 4 shows the results of a simulation of this algorithm. Figure 4(a) is the same 
starting distribution as in Figure 3(a). Figures 4(b) through 4(e) show some selected 
intermediate distributions while Figure 4(f) illustrates the final distribution. 

Some comments on the algorithm are in order. As mentioned earlier, the potential 
field algorithm is utilized to avoid collision. If d 0 is used to denote the cut-off distance of 
repulsive forces in the potential field algorithm, any obstacles (in this case other robots) 
which are less than d 0 distance away from robot R will generate repulsive forces to robot 
R. In Step 3, when determining if there is enough room to fit another robot between R c 
and Rf the distance from R c and 7?/must be at least: 

R c R f \ = 2r 0 + 2d 0 , ( 14 ) 

where r Q is the radius of the robots. (A Nomad 200 robot is cylindric in shape. In the 
simulator, it is represented by a circle.) That is, if there is at least 2 r Q + 2 d a distance 
between R c and Rf robot R will be able to "squeeze" in. This is true even if there are 
other robots between R c and Rf 
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Figure 4. Selected images of the modified line algorithm simulation: (a) the initial 
distribution, (b)-(e) intermediate steps, and (f) the final distribution of the robots 
forming a line segment. 


















In Step 4, the distance d is given by 

d = 2r 0 +d 0 . ( 15 ) 

There is, however, a problem in implementing Step 4 if point p is between R c and 
Rf. Sending robot R directly to pd mostly results in a local minimum while the robot tries 
to move to the other side of R c . This is inevitable if the robot's direct route to pd is within 
the repulsive force range of R c . This problem is avoided by sending the robot to an 
intermediate point which is d distance away from R c , on the line that is perpendicular to / 
at R c . There are two points on this perpendicular line which are d 0 distance away from R c , 
but there is an obvious choice, the one which is closer to robot R. As soon as robot R 
reaches this intermediate point within a close proximity, its goal point is changed to pd. 

If robot R detects only one robot nearby (which is the case if it is at the endpoint 
of the line segment), it positions itself d 0 distance away from the detected robot. If robot 
R does not detect any robots nearby, it can execute an algorithm to search for possible 
existence of other robots, which is not in the scope of this study. 

Finally it is noted that all robots will uniformly distribute in the line segment 
because each robot tries to go to the mid-point between its neighbors until it gets into the 
repulsive force range of its neighbors. 
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IV. FORMATION OF A CIRCLE 


This chapter first discusses the existing circle formation algorithm, and simulation 
results from implementing the existing algorithm. Observing problems encountered with 
the existing algorithm, a sequence of modification and improvement is proposed. The 
improvement is directed towards forming a better approximation of a circle while 
uniformly distributing robots on a circle. 

A. EXISTING CIRCLE ALGORITHM 

Let robot R be any one of the distributed robots participating in the task of circle 
formation. The existing circle algorithm works as follows [Ref. 5]. As before, robot R 
becomes active and inactive at random instants of time. Each time robot R becomes 
active, it: 

• Step 1. Determines the furthest robot Rf and closest robot R c . 

• Step 2. Calculates the distance d from its current position to the middle 

point p m between Rf and R c . 

• Step 3. Moves a distance of min{d-r, v} towards p m if (d-r) >— 0, or a 
distance of minfr-d, vj away from p m if (d-r) < 0, where v is the maximum 
distance that a robot can move at a time, r is the desired radius of a circle to 
be formed. 

Figure 5 shows the results of a simulation of this algorithm. The desired radius of 
the circle is 28.0 inches. (The radius of the Nomad robot is 9.0 inches.) Figure 5(a) is the 
initial, random distribution of robots. Figure 5(b) to (e) show the intermediate positions 
of robots, and Figure 5(f) illustrates the final stage of the simulation. The final 
distribution of robots is a good approximation of a circle, and robots are fairly uniformly 
distributed. However, the degree of uniformity depends on the number of robots. Figure 
6(a) shows the final distribution of five robots. The distribution is apparently less 
uniform. 
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Figure 5. Selected images from a simulation of the existing circle algorithm 
implemented by using the potential field method: (a) the initial distribution, (b)-(e) 
intermediate steps, and (f) the final distribution of the robots forming a circle. 
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Figure 6. The final distributions of two more simulations of the existing circle 
algorithm started from the same initial distribution as the earlier simulation: (a)If a 
robot is missing, the remaining robots still form a circle, but are not uniformly 
distributed (r=28 inches), (b) The robots fail to form a circle for a relatively large 
desired radius (r=120 inches). 


A minor problem is that the radius of the final circle is always smaller than the 
desired radius (20 inches versus 28 inches in this case). This is because p m does not 
correspond to the origin of the circle. Consequently, the final formation appears as two 
half-circles put together. In Figure 5(f), the three lower-left robots form a half-circle as 



















do the three upper-right robots. This is the same source that causes robots to form a 
Reuleaux's triangle [Ref. 1, 4, 5]. 

Another problem occurs when the desired radius becomes relatively large. With 
limited sonar range, a robot is not able to see some robots as it moves outwards to form a 
large circle. With the same initial distribution as in Figure 5(a) and Figure 6(a), a 
simulation is carried out to form a circle with radius of 120 inches. The resulting 
distribution is shown in Figure 6(b). The pair of robots at the upper-right comer cannot 
see the two at the lower-left comer due to limited sensor range. In this case, the two 
robots at the upper-right comer and the two in the middle form a circle. Similarly, the 
two robots at the lower-left comer and the two in the middle form another circle. 

Finally, it is noted that algorithm works the same way but much faster when it is 
implemented without using any sleep mode (making the robots active or inactive at 
unpredictable time instants). 

B. MODIFIED CIRCLE ALGORITHM 

In this subsection, a modified circle algorithm is presented. The objective of the 
modified algorithm is to yield a better approximation of circles, and to uniformly 
distribute robots. The existing algorithm utilizes position information of the furthest and 
closest robots. It is attempted to improve it by utilizing position information of one more 
robot. More specifically, the steps of the modified algorithm are as follows. At each 
iteration, robot R: 

• Step 1. Determines the furthest robot R f , the closest robot R cJ , and the 
second closest robot R c2 . 

• Step 2. Computes the coordinates of the centroid p m of R f , R cl , and R c2 . 

• Step 3. Moves to the point p r which is r distance away from p m , on the line 

that passes through its current position and p m , where r is the desired radius of 
a circle to be formed. 


28 



Figure 7. Selected images of a simulation of the modified circle algorithm: (a) the 
initial distribution, (b)-(e) intermediate steps, and (f) the final distribution of the 
robots forming a circle. 
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In step 3, while robot R tries to move towards or away from p m in order to take its 
position on p r , it is pushed by R c and R c2 , which makes robot R move to the sides. An 
advantage of the modified algorithm is that p m is closer to the origin of the desired circle, 
which makes the final formation a much better approximation of a circle. The radius of 
the resulting circle is still smaller than the given radius (21 inches versus 28 inches). 

Another advantage is that robots will be uniformly distributed along a circle, 
independent of the number of robots. Nevertheless, it is observed that smaller number of 
robots tend to form a smaller circle. Figure 8 shows the final results with five and four 
robots. 


• # 


(a) 



(b) 


Figure 8. Final distributions of the modified circle algorithm simulations with five 
and four robots. 


30 










V. 


MOVING IN FORMATION 


In the previous chapters, the problem of formation was addressed. This chapter 
discusses a rather different problem: moving in formation. Starting from an initial 
formation, the problem of moving in formation is concerned with how to move robots 
from an initial location to a final goal location while maintaining the formation and 
avoiding obstacles. If the initial formation cannot be maintained due to obstacles, robots 
must stay in a formation as close as possible to the initial one. 

A. ASSUMPTIONS 

Additional assumptions made for this section are : 

• There is a global coordinate system, and the final goal location is passed to 
each robot. 

• There is a bulletin board that can be reached by all the robots. Each robot is 
assigned a specific location in the bulletin board and each robot writes its 
position information always into its assigned location. 

B. IMPLEMENTATION 

Three robots are used in implementation in this section as Nserver does not show 
100 percent reliability with larger number of robots in implementation of the bulletin 
board communication scheme. "Leader-Follower" technique is chosen for formation 
position determination. In this technique, each robot determines its formation position 
relative to the leader robot. The leader simply moves towards the final goal location 
independently while other robots try to maintain their positions in the formation relative 
to the leader. The robots are all identical and the leader is not appointed, but it is selected 
by the robots at run time. 

At their initial locations, the robots only know the final goal location. Each one 
writes its current position information into its assigned location in the bulletin board. 
Then it calculates the distances of all robots from the final goal location, based on the 
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position information it reads from the other robots' assigned locations in the bulletin 
board. The closest one to the final goal location is selected as the leader. The location 
number in the bulletin board which consists of the position information of this robot is 
memorized by all robots; they know where the leader's position information is available. 
Once the leader is selected, only the leader keeps writing its position to the bulletin board 
in every iteration. The other robots do not need to do it as their position information do 
not need to be known by any others. 

Each robot determines its position relative to the leader and remembers it 
throughout the whole journey. The first movement for all the robots is to turn their faces 
towards the final goal location. This is to find out which side is the leader located. Each 
follower robot needs to know if it is on the left or on the right side of the formation as to 
the direction of motion. For instance, if a robot sees the leader on its right side initially, 
then it will move towards the formation's main body by leaving the obstacle on his left 
when avoiding collisions. If the leader is initially on its left, then it will leave the obstacle 
on its right and move towards the formation's main body when avoiding obstacles. 

The leader moves towards the given final goal location independently, while 
avoiding any collision based on the potential field method. Each of the other robots 
determines a new goal location for itself at each iteration based on the current position of 
the leader and moves to this point while avoiding any collision, again based on the 
potential field method. The follower robots move slightly faster than the leader in order 
to catch it and maintain the formation. The further the follower robot is from its goal 
location at each iteration, the faster it moves. This is very crucial because the followers 
stay behind when avoiding collisions with obstacles while the leader keeps moving. 

It can be explained better using Figure 9 which shows the simulation results from 
an initial to a final distribution. The final location for the robots in Figure 9(a) is given to 
be a point which is further on the right side of the area shown. The robots concur with 
the leader, they face towards the final goal point, the followers find out which side is the 
leader located, and determine their positions relative to the leader. Then, the leader starts 
moving towards the final goal point while the others start following it. Since the robot on 
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the very right in Figure 9(a) is the closest one to the final destination, it will be the leader. 
(Let's denote the leader as Rl, the robot on the top side as Rj, and the robot on the bottom 
side as Rb ). 

In Figure 9(b), Rt comes across an obstacle. After having a very slow speed (2 
inches/sec. in simulations) two successive times although it is away from its goal 
location, it realizes that it is stuck and turns towards the main body of the formation and 
leaves the obstacle on its left. Then it keeps moving while keeping the obstacle on its left 
hand side. This makes the robot follow the edge of the obstacle and turn around the 
comer of the obstacle. As soon as Rt realizes that it moved towards the final goal 
location more than 10 inches in one iteration, it goes back to the normal flow of the 
program, determines a new goal position for itself relative to the current position of the 
leader, and starts moving. The new goal location may cause the robot to immediately 
experience another local minimum, especially if the avoided obstacle is a big one, which 
makes the robot go to the same process one more time. 

The follower robots move faster when they are avoiding an obstacle. Otherwise 
they would stay very much behind the leader. Rl avoids obstacles always by leaving them 
on its right and moving left as to the direction of general motion of the formation, as 
appeared on Figure 9 (c) and (d). Rb avoids the obstacles by leaving them on its right and 
moving towards the main body of the formation as shown on Figure 9 (h) and (i). 
Moving to the opposite direction would cause the follower robots go further away from 
the formation, which in turn makes it difficult to catch. 

The bulletin board scheme is implemented by using a built in function called 
g e t_rpx0 which is not specified in the language manual but included in Nclient.h and 
Nclient.o files. This function uses the UNIX sockets to communicate with the server and 
get the position information of all the nearby robots. It has been a little bit difficult to 
figure out how exactly it worked as it was not stated in the language manual. Therefore, 
it is believed to be useful to include it here, in order to ease the understanding of the 
source code. 
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Figure 9. Selected images of the simulation of the moving in formation algorithm 
from an initial distribution to a final stage, (a) the initial distribution, the robot on 
the very right is selected to be the leader, (b)-(f) and (g)-(k) on the next page are 
intermediate steps, 
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Figure 9. Continued. Leader moves independently towards the final goal location 
while others try to maintain their positions relative to the leader while avoiding 
obstacles, and (1) the distribution of the robots after avoiding all the obstacles and 
shortly before arriving the final goal location. Three identical small circles 
represent the robots, while big polygons represent obstacles. 
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A pointer is passed to the function which in return points to the position 
information of the nearby robots. The first value in the returned array (pos_array[0 ]) 
specifies the number of robots around. Each following group of three values in the array 
returns information about a robot. The first ones in each group of three (pos_array[l ], 
pos_array[4], pos_array[7], etc.) specifies the robot's ID number. The following two 
values in each group contain the x and y coordinates of that robot (i.e. pos_array[2], 
pos_array[5 ], pos_array[8] contain the x-coordinate, and pos_array[3], pos_array[6], 
pos_array[9] contain the y-coordinate of the robot whose number is specified in 
pos_array[l ], pos_array[4J, pos_array[7] respectively). The x and y coordinates of 
each robot are presented relative to the robot itself in the world coordinate system. For 
instance, if the robot who is calling this function is located at x=100, y=50 in the world 
coordinate system, and if there is only one robot around whose ID number is 5 and who is 
located at x=350, y=200 in the world coordinate system, then the function returns the 
following values: rob_pos[0]=l, rob_pos[l]=5, rob_pos[2]=250, rob_pos[3]=150. 

The function get_rpxQ does not return any information about the robots that are 
not in the sensor range or are completely behind an obstacle. If a follower robot cannot 
get the position information of the leader in such cases, which is encountered 
occasionally, it simply moves towards the final goal position with a higher speed until it 
gets the position information of the leader again. 

Any robot can turn backwards while avoiding a collision or trying to maintain its 
position and carry on moving backwards later on as it happens to the leader on Figure 9 
(g) and (h). This is a natural result of using physical robots, and it does not affect the 
flow of the motion at all. Robots repeat the motion sequences as appeared in the rest of 
the Figure 9 and maintain the initial formation as much as possible while avoiding 
obstacles on the way to the specified final goal configuration. 
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VI. CONCLUSIONS AND RECOMMENDATIONS 


Line and circle formation of distributed mobile robots was studied. Motion 
control and collision avoidance were achieved by implementing a potential field 
algorithm. It was observed that existing line and circle formation algorithms do not work 
well when implemented with realistic robots. Modified line and circle algorithms were 
developed and verified through simulation. As demonstrated, the proposed algorithms 
not only achieve the goal of forming a line or a circle, but they also uniformly distribute 
robots on a line segment or circle. Behaviors for formation maintenance in a small group 
of physical mobile robots was also studied. An algorithm based on “Leader-Follower” 
technique was implemented for relative positional maintenance to help robots move to a 
specified goal location in formation while avoiding collisions with obstacles and other 
robots. 

Follow-on work on the line and circle formation problems should focus on 
improving the convergence rate of formation and incorporating obstacles (other than 
robots themselves) in workspace. Studies on the moving-in-formation problem, on the 
other hand, should focus on implementing a means of implicit communication in which 
robots rely entirely upon their own perceptions of the world. 
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APPENDIX A. SOURCE CODE FOR MODIFIED LINE ALGORITHM 


/* This is the source code of the modified line algorithm presented in 
chapter III. Some command line arguments must be passed to the program 
in order to run it. The first argument is the ID number of the robot 
that will be used to connect to the server, which is compulsory. Second 
and third arguments are the desired initial x and y coordinates of the 
robot, and these are optional. If the current position of the robot is 
desired to be the initial configuration, then no x-y coordinates need to 
be entered. So, there are two ways to run the program. 

1. Filename ID x y 

2. Filename ID 

One copy of this program must be run for each robot with a different ID 
numbers. Nserver allows to simulate up to six robots, hence the robot 
ID number should be between 1 and 6. */ 

#include "Nclient.h" 

#include <stdio.h> 

#include <stdlib.h> 

#include <math.h> 

#define PI 3.1415926 

/* Function Prototypes */ 
void GetSensorData(void); 
void Movement(void); 
int sign(int); 
void calculate() ; 

/* Global Variables */ 

double fused_range[16]; /* Array to store the fused sensor readings */ 
int BumperHit = 0; /* boolean value */ 

long robot_config[4]; /* the current robot configuration(x, y, 

steering angle, turret angle) x and y are in 
tenth of inches, and angles are in tenth of 
degrees */ ■ 

double F_att[2], F_rep[2], F_tol[2] ; /^Arrays to store the attractive, 

repulsive and total forces. */ 

double preXgoal, preYgoal ; 
int minreturn, maxreturn, iteration ; 
long mindist, maxdist ; 
int count = 0 ; 

/*** Main Program ***/ 

main (unsigned int argc, char* argv[]) 

{ 

int i,index; 
int order[16]; 

int Robot_ID = atoi(argv[1]); 
int X,Y; 
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/* Check the command line arguments. There should be either one or 
three command line arguments */ 
if (argc!=4) { 

if (argc!=2) { 

printf("please enter 3 parameters besides the command\n") ; 
exit(); 


} 

/* Check the robot ID, IT should be between 1 and 6 */ 
if (.(Robot_ID<l) || ( Robot_ID>6) ) { 

printf("Robot ID must be between 1 and 6 ") ; 
exit (); 

} 

/* Connect to Nserver.*/ 

SERV_TCP_P0RT=7 771 ; 

strcpy(ROBOT_MACHINE_NAME, "nomad"); 

connect_robet(Robot_ID); 

/* initial x-y coordinates are entered, store them into 

variables*/ 

if (argc==4) { 

X = atoi(argv[2]); 

Y = atoi(argv[3] ) ; 
place_robot (X,Y,0,0); 

} 


/* initialize Smask and send to robot. Smask is a large array that- 
controls which data the robot returns back to the server. This 
function tells the robot to give us everything. */ 
init_mask(); 

/* Configure timeout (given in seconds). This is how long the robot 
will keep moving if it becomes disconnected. */ 
conf_tm(5); 

/* Sonar setup: configure the order in which individual sonar units 
^ re * this case, fire all units in counter-clockwise order (units 
are numbered counter-clockwise starting with the front sonar as zero) 
The conf^sn() function takes an integer and an array of at most 16 
integers. If less than 16 units are to be used, the list must be 
terminated by an element of value -1. The single integer value passed 
controls the time delay between units in multiples of four 
milliseconds. */ 
for (i = 0; i < 16; i++) 
order[i] = i; . 
conf_sn(1,order); 


/* Configure the Infrared Sensors */ 
for (i = 0; i < 16; i++) 
order[i] = i; 

conf_ir(1, order); 

/* Fortunately, the robot can talk... */ 
tk("Start to form a line"); 
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iteration = 0 ; 


/* Main loop. Execute unless a collision occurs. */ 
while (IBumperHit) 

{ 

GetSensorData(); 

Movement (); 

} 


/* Disconnect. */ 
disconnect robot(Robot ID) ; 


/******************************/ 

/* Movement(). This function is responsible for using the sensor data to 
direct the robot's motion appropriately. */ 
void Movement (void) 

{ 

/* Variables */ 
int i ; 

int panic,flip ; 
int tvel, svel ; 

double gain_tvel = 0.07; /* Translational velocity gain */ 

double gain_svel = 100.0; /* Rotational velocity gain */ 

/* Compute the attractive force and the repulsive force in the robot 
coordinates by using function calculate() */ 
calculate () ; 

/* Set the translational velocity */ 
tvel = (int) (gain_tvel * F_tol[0]); 

/* Set the rotational velocity. If both minimum and maximum sensor 
readings are 255 which is the maximum sonar range, it means there is 
no contact around. Then proceed by turning 5 degrees in each 
iteration, so that robot will be able to cover the area by making a 
big circle. Otherwise set the rotational velocity based on the total 
forces acting on robot. */ 

if(mindist==255 && maxdist==255) 
svel =50 ; 

else 

svel = (int) (gain^svel * sin(atan2(F_tol[1],F_tol[0])));' 

/* Set the direction of rotation;left or right. */ 
svel = svel * sign( (int) (F_tol[0]) ); 

/* limit the translational and rotational velocities. Maximum allowed 
translational velocity is 24 in/sec, and rotational velocity is 45 
degrees. */ 
if (abs(tvel) > 240) 

tvel = 240 * sign(tvel); 

if (abs(svel) > 450) 

svel = 450 * sign(svel) ; 


41 


/* the r °kot's velocities. The first parameter is the robot's 
translational velocity, in tenths of an inch per second. This velocity 
can be between -240 and 240. The second parameter is the steering 
velocity, and the third is the turret velocity. The units of the 
latter two are tenths of a degree per second, and can be between -450 
and 450. The same value is given for these two so that the turret is * 
always facing the direction of motion. */ 
vm(tvel,svel,svel); 


} 




/* This function reads sensor data and 
void GetSensorData (void) 

{ 

int i ; 

long SonarRange[16] ; /* Array 

long IRRange[16] ; /* Array 

double corrected__IR[16] ; /* Array 

readings 

double corrected_jsonar [16] ;/* Array 

readings 

double norm[16] ; 


loads them into arrays. */ 


to store sonar readings */ 
to store infrared readings */ 
to store correlated infrared 
*/ 

to store the corrected sonar 
*/ 


/* R ead all sensors and load data into State array. */ 
gs () ; 


/* Read State array data and put readings into individual arrays. */ 
for (i = 0; i < 16; i++) { 

/* Sonar ranges are given in inches, and can be between 6 and 255, 
inclusive. */ 

SonarRange[i] = State[17+i]; 

/* IR readings are between 0 and 15, inclusive. This value is 
inversely proportional to the light reflected by the detected 
object, and is thus proportional to the distance of the object. 

Due to the many environmental variables effecting the reflectance 
of infrared light, distances cannot be accurately ascribed to the 
IR readings. */ 

IRRange[i] - State [1+i]; 

} 

/* To correlate the infrared reading to physical distance. The 
numbers are obtained by least square linear regression of measurement 
data. */ 

for (i = 0; i < 16; i++) 

corrected_IR[i] = 2.2508 * ((double) IRRange[i] + 0.8602); 
for (i = 0; i < 16; i++) 

corrected_sonar[i] = (double) SonarRange[i]; /*From long to double*/ 

/* Ruse the sonar and IR data, and store the final sensor reading into 
the global array fused__range [ ] . Infrared readings are not reliable 
beyond 14 inches. If the correlated value is smaller than 14, then 
normalize and fuse the infrared and sonar data. If it is more than 
14, then consider the sonar data. */ 
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for (i = 0; i < 16; i++) { 

if (IRRange[i] <= 14) 

{ 

norm[i] = corrected_sonar[i]*corrected_sonar[i] + 
corrected_IR[i]* correct ed__IR [i]; 
fused_range [i] = (corrected_sonar[i]* corrected_sonar[i]* 
corrected__IR[i] + corrected_IR[i] * 
corrected__IR[i] * corrected_sonar [i] ) /norm[i] ; 
if (fused_range[i] <= 5.0) 
fused_range[i] = 0.0; 

} 

else 

fused__range [i] = corrected__sonar [i] ; 

1 

/* The robot configuration parameters (x, y, steering angle, and 
turret angle) are stored in State[34], State[35], State[36], and 
State[37]. */ 

for (i =0; i < 4; i++) 

robot_config[i] = State[34+i]; 

/* Check for bumper hit. If a bumper is activated, the corresponding 
bit in State[33] will be turned on.Since we don f t care which bumper is 
hit, we thus only need to check if State[33] is greater than zero. */ 
if (State[33] > 0) { 

BumperHit = 1; 
tk Touch.”) ; 

printf("Bumper hit!\n”); 

} 

/* Find out the sensor ID number which detects the closest robot Rc */ 

minreturn =0; 

for (i = 1 ; i < 16 ; i++) { 

if (fused_range[i] < fused_range[minreturn]) 
minreturn = i ; 

} 

/* The distance to the closest robot */ 
mindist = fused_range[minreturn]; 

/*Find out the sensor ID number which detects the furthest robot Rf*/ 

maxreturn = minreturn ; 

for (i=0;i<16; i++) { 

if((fused_range[i]>=fused__range[maxreturn])&&(fused_range[i]<255.0) ) 
maxreturn = i; 

} 

/* The distance to the furthest robot */ 
maxdist = fused_range[maxreturn]; 

} 

/**-k'k*'k'k'k***'k'k*'k-k±-k-k*-k-k*********/ 

/* Sign function. It returns 1 if x is positive, and returns -1 
otherwise */ 
int sign(int x) 

{ 

return x>0?l:-l; 

} 
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/* This function computes the attractive force as to the goal point in 
robot coordinate system and the repulsive force as to the obstacles in 
robot coordinate system. Finally it calculates the x and y components of 
the total forces on the robot in robot coordinate system. */ 


void calculate() 

{ 

double rho_0 = 40.0; /* 

double scale =8.0 ; 
double eta = 35000.0 ; 
double rho_att = 40.0 ; 


Cut-off distance of the repulsive force */ 

/* Scaling factor for attractive force */ 
/* Scaling factor for repulsive force */ 

/* Saturation distance for attractive 
force to switch between parabolic-well and 
conic-well */ 


double rho_float ; 
double teta; 

double range = (2*rho_0) + (2*8.81) ; /* Minimum distance between 

closest and furthest robot to 
allow another robot to fit in.*/ 

int i ; 

int signal - 0 ; 
int flag = 0 ; 
int set = 0 ; 
int alone = 0 ; 


double xxl,xx2,yyl,yy2 ; 
double length, slope, dist, distance ; 
double xgoal, ygoal ; 
double ya,yb,xa,xb ; 


/* The x and y coordinates of the closest robot */ 

xxl = ((double)mindist+17.62)*cos((double)minreturn*0.39) ; 

yyl = ((double)mindist+17.62)*sin((double)minreturn*0.39) ; 

/* The x and y coordinates of the furthest robot */ 

xx2 = ((double)maxdist+17.62)*cos((double)maxreturn*0.39) ; 

yy 2 = ((double)maxdist+17.62)*sin((double)maxreturn*0.39) ; 

/* The distance between closest and furthest robots */ 
length = hypot ( (double)(yy2-yyl),(double)(xx2-xxl) ); 

/* Compute the coordinates of the point p, the foot of the perpendi¬ 
cular drop from robot's current position to the line 1 passing through 
Rc and Rf. Watch out the conditions that would lead to a division by 
zero error. */ 
if ( xxl — xx2 ) { 

if (yyl == yy2) { /*Means there is only one contact around*/ 

dist = hypot((double)xxl,(double)yyl) ; 
printf("dist= %f\n", dist); 
xgoal = xxl - (xxl*range/dist) ; 
ygoal = yyl - (yyl*range/dist) ; 
alone = 1 ; 

} 

else { 

xgoal = xxl ; 
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ygoal = 0 ; 

signal = 1 ; 

} 

} 

else { 

if <yyl==yy2) { 
xgoal = 0 ; 
ygoal = yyl ; 
signal = 2 ; 

} 

else { 

slope = (yy2-yyl)/(xx2-xxl) ; 

xgoal = ( yyl- (slope*xxl)) / ((-1/slope) - slope ); 
ygoal = (-1/slope)*xgoal ; 
distance = hypot(xgoal,ygoal) ; 
signal = 3 ; 

} 

} 

/* Check to see if the point p is between Rc and Rf. If so, there is 
enough room for a robot to fit in-between Rc and Rf. Then proceed 
towards the mid-point. */ 

if (((xgoal>=xxl)&&(xgoal<=xx2))||((xgoal<=xxl)&&(xgoal>=xx2))) { 

if (((ygoal>=yyl)&&(ygoal<=yy2))||((ygoal<=yyl)&&(ygoal>=yy2))) { 

set = 1 ; 

if (length >= range) { 

xgoal = (xxl+xx2)/2.0 ; 
ygoal = (yyl+yy2)/2.0 ; 
flag - 1 ; 
set = 0 ; 

} 

} 

} 

/* If point p is not between Rc and Rf, or if there is not enough 
room between Rc and Rf, then the final goal location is the point pd 
on the line 1 which is d distance away from Rc in the opposite 
direction of Rf, where d is the distance that would prevent any 
repulsive force being applied to either robot. */ 
if (!flag) { 

switch(signal) { 

case 0 : break ; 

case 1 : ygoal = sign(yyl)*(abs(yyl)-sign(yyl)*sign(yy2)*range) 
break ; 

case 2 : xgoal = sign(xxl)*(abs(xxl)-sign(xxl)*sign(xx2)*range) 
break ; 

case 3 : xa = xxl + (range-rho_0)/sqrt(l+slope*slope) ; 

xb = xxl - (range-rho_0)/sqrt(l+slope*slope) ; 
ya = yyl + slope*(xa-xxl) ; 
yb - yyl + slope*(xb-xxl) ; 

if ( (hypot(ya,xa)-hypot(yb,xb))<=0.0 ) { 

xgoal = xa ; 
ygoal = ya ; 

} 

else { 

xgoal = xb ; 
ygoal = yb ; 



} 

break ; 
default: break ; 

} 

} 

/* ^ point p is between Rc and Rf but there is not enough room for 
between Rc and Rf, proceeding directly to pd mostly results in local 
minima when the direct route to pd is within the repulsive force 
range of Rc. To avoid this problem, send the robot to an intermediate 
point which is d distance away from Rc, on the line that is 
perpendicular to 1 at Rc. As soon as the robot passes this point, in 
the next iteration pm will not be between Rc and Rf and robot will be 
sent to pd. */ 
if (set && !alone) { 

ya = yyl + (range-rho_0)/sqrt(l+slope*slope) ; 
yb = yyl - (range-rho_0)/sqrt(l+slope*slope) ; 

xa = xxl + slope*(yyl-ya) ; 
xb = xxl + slope*(yyl-yb) ; 
printf("ya,yb, loop executed\n")/ 
if ( (hypot(ya,xa)-hypot(yb,xb))<=0 ) { 
xgoal = xa ; 
ygoal = ya ; 

} 

else { 

xgoal = xb ; 
ygoal = yb ; 

} 


/* If Rc and Rf are seen by two consecutive sensors and the 
difference between the sensor readings are less than 4 inches, then 
there is only one contact around that is seen by two sensors. This 
happens when the endpoint robots are moving to their positions, 
after a certain point the robot cannot see the other robots. In this 
case use the last goal values three times to send the robot to its 
correct position, in order to form a straight line. Then simply send 
the robot to the d distance away from the only detected robot. */ 
if ((abs(minreturn-maxreturn)==1 )||(abs(minreturn-maxreturn)=-15)){ 
if ( abs(mindist-maxdist)<=4 ) { 

if (count < 3 ) { 

xgoal = preXgoal ; 
ygoal = preYgoal ; 
count += 1 ; 

} 

else { 

dist = hypot((double)xxl,(double)yyl) ; 
xgoal = xxl - (xxl*range/dist) ; 
ygoal = yyl - (yyl*range/dist) ; 
count = 0 ; 

} 

} 

} 

distance = hypot(xgoal,ygoal); /*distance to the goal configuration*/ 

/* Store the goal configuration to remember in the next iteration */ 
preXgoal = xgoal ; 
preYgoal = ygoal ; 
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/* Compute the attractive force in the robot coordinate system */ 
if (distance <= rho_att) { /* Parabolic Well */ 

F_att[0] = scale*xgoal ; 

F_att[l] = scale*ygoal ; 

} 

else { /* Conic Well */ 

F_att[0] = scale*rho_att*(xgoal/distance) ; 

F_att[l] = scale*rho_att*(ygoal/distance) ; 

printf ( "distance2= %f \n ,r f distance); 

} 

/* Compute the repulsive force in the robot coordinate system */ 
F_rep[0] = 0.0; 

F_rep[l] = 0.0; 

for (i = 0; i <= 15; i++) 

{ 

rho_float = (double) (fused_range[i] ) ; 
if (rho_float < rho_0) { 

F_rep[0] 4-= -eta* (1.0/rho_f loat - 1.0/rho_0) *cos ( (double) (i) * 

0.392699)/(rho_float) ; 

F_rep[l] += -eta*(1.0/rho_float - 1.0/rho_0)*sin((double)(i) * 
0.392699)/(rho_float); 

} 

} 

/* compute the total force in the robot coordinates */ 

F_tol [0] = F_att[0] + F_rep[0]; 

F tol[1] = F att [ 1] + F_rep[1]; 


} 
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APPENDIX B. SOURCE CODE FOR MODIFIED CIRCLE ALGORITHM 


/* This is the source code of the modified circle algorithm presented 
in chapter IV. Some command line arguments must be passed to the 

program in order to run it. The first argument is the ID number of the 

robot that will be used to connect to the server, which is compulsory. 

Second and third arguments are the desired initial x and y coordinates 
of the robot, and these are optional. If the current position of the 
robot is desired to be the initial configuration, then no x-y 

coordinates need to be entered. So, there are two ways to run the 
program. 

1. Filename ID x y 

2. Filename ID 

One copy of this program must be run for each robot with a different ID 
numbers. Nserver allows to. simulate up to six robots, hence the robot 
ID number should be between 1 and 6. */ 

#include "Nclient.h" 

#include <stdio.h> 
tinclude <stdlib.h> 

#include <math.h> 

#define PI 3.1415926 

/* Function Prototypes 
void GetSensorData(void 
void Movement(void); 
int sign(int); 
void calculate() ; 

/* Global Variables */ 
long SonarRange[16]; 
long IRRange[16]; 
double fused_range[16]; 
int BumperHit = 0; 
long robot_config[4]; 


double radius = 28.0 ; 
double F_att[2], F_rep[2], F_tol[2]; /^Arrays to store the attractive, 

repulsive and total forces. */ 
int minreturn, maxreturn, minreturn2 ; 
int count = 0 ; 

long mindist, mindist2 ,maxdist; /* Variables to store the distances 

of closest, second closest and 
furthest robots */ 


*/ 
); 


/* array of sonar readings (inches) */ 

/* array of infrared readings (no units)*/ 

/* Array to store the fused sensor readings */ 
/* boolean value */ 

/* the current robot configuration (x, y, 
steering angle, turret angle) x and y are in 
tenth of inches, and angles are in tenth of 
degrees */ 


/*** Main Program ***/ 

main (unsigned int argc, char* argv[]) 

{ 

int i,index; 
int order[16]; 

int Robot_ID = atoi(argv[1]); 
int X,Y; 
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/ Check the command line arguments. There should be either one or 
three command line arguments */ 
if (argc!=4) { 

if (argc!=2) { 

printf{"please enter 3 parameters besides the command\n") ; 
exit (); 

} 

} 

/* Check the robot ID, It should be between 1 and 6 */ 
if ( (Robot_ID<l) | | ( Robot__ID>6) ) { 

printf("Robot ID must be between 1 and 6 "); 
exit () ; 

} 


/* Connect to Nserver.*/ 

SERV_TCP_PORT=7771 ; 

strcpy(ROBOT JV1ACHINEJJAME, "nomad"); 

connect_robot(Robot_ID); 

/* ^ initial x-y coordinates are entered, store them into 

variables. */ 

if (argc==4) { 

X = atoi(argv[2]) ; 

Y = atoi(argv[3]); 
place__robot(X,Y,0,0); 

} 


/ Initialize Smask and send to robot. Smask is a large array that 
controls which data the robot returns back to the server. This 
function tells the robot to give us everything. */ 
init_mask(); 

Configure timeout (given in seconds). This is how long the robot 
will keep moving if it becomes disconnected. */ 
conf_tm(5); 

/* Sonar setup: configure the order in which individual sonar units 
fire. In this case, fire all units in counter-clockwise order (units 
are numbered counter-clockwise starting with the front sonar as zero) 
The conf_sn() function takes an integer and an array of at most 16 
integers. If less than 16 units are to be used, the list must be 
terminated by a element of value -1. The single integer value passed 
controls the time delay between units in multiples of four msec. */ 
for (i = 0; i < 16; i++) 
order[i] = i; 

conf_sn(1, order) ; 

/* Configure the Infrared Sensors */ 
for (i = 0; i < 16; i++) 
order[i] = i; 

conf_ir(1,order); 

/* Fortunately, the robot can talk... */ 
tk("Let's form a circle guys"); 
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/* Main loop. Execute unless a collision occurs. */ 
while (!BumperHit) 

{ 

GetSensorData{); 

Movement(); 

} 


/* Disconnect. */ 
vm (0,0,0) ; 

disconnect robot(Robot ID) ; 


/******************************/ 

/* Movement(). This function is responsible for using the sensor data to 
direct the robot 1 s motion appropriately. */ 

void Movement (void) 

{ 

/* Variables */ 
int i ; 
int panic; 
int tvel, svel; 

double gain_tvel =0.07; /* Translational velocity gain */ 

double gain_svel = 100.0; /* Rotational velocity gain */ 

/* Compute the attractive force and the repulsive force in the robot 
coordinates by using function calculate() */ 

calculate() ; 

/* set the translational velocity */ 
tvel = (int) (gain_tvel * F_tol[0]); 

/* Set the rotational velocity. If both minimum and maximum sensor 
readings are 255 which is the maximum sonar range, it means there is 
no contact around. Then proceed by turning 5 degrees in each 
iteration, so that robot will be able to cover the area by making a 
big circle. Otherwise set the rotational velocity based on the total 
forces acting on robot.*/ 

if(mindist==255 && maxdist==255){ 
svel = 50 ; 
tvel = 100 ; 

} 

else { 

if ((F_tol[0]==0)&&(F_tol[l]==0)) 
svel = 0 ; 

else if ( F_tol[0]==0 ) 
svel = 450 ; 
else 

svel = (int) (gain_svel * sin(atan2(F_tol[1], F_tol[0]) ) ) ; 

} 

svel = svel * sign( (int) (F_tol[0]) ); 
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/* limit the translational and rotational velocities. Maximum allowed 
translational velocity is 24 in/sec, and rotational velocity is 45 
degrees/sec */ 
if (abs(tvel) > 240) 

tvel = 240 * sign(tvel) ; 

if (abs(svel) > 450) 

svel = 450 * sign(svel) ; 

/* Set the robot *s velocities. The first parameter is the robot T s 
translational velocity, in tenths of an inch per second. This velocity 
can be between -240 and 240. The second parameter is the steering 
velocity, and the third is the turret velocity. The units of the 
latter two are tenths of a degree per second, and can be between -450 
and 450. The same value is given for these two so that the turret is 
always facing the direction of motion. */ 
vm(tvel,svel,svel); 


/*******************************y 

/* This function reads sensor data and loads them into arrays. */ 
void GetSensorData (void) 

{ 

int i, value , samel, same2 ; 

double corrected_IR[16] ; /*Array to store correlated infrared 

readings. */ 

double corrected_sonar[16]; /* Array to store the corrected sonar 

readings */ 

double norm[16] ; 

int min_above, min_below ; 

/* Read all sensors and load data into State array. */ 
gs () ; 

/* Read State array data and put readings into individual arrays. */ 
for (i = 0; i < 16; i++) ' 

{ 

/* Sonar ranges are given in inches, and can be between 6 and 255, 
inclusive. */ 

SonarRange[i] = State[17+i]; 

/* IR readings are between 0 and 15, inclusive. This value is 
inversely proportional to the light reflected by the detected 
object, and is thus proportional to the distance of the object. 

Due to the many environmental variables effecting the reflectance 
of infrared light, distances cannot be accurately ascribed to the 
IR readings. */ 

IRRange[i] = State[1+i]; 

} 


/* correlate the infrared reading to physical distance. The numbers 
are obtained by least sguare linear regression of measurement data. */ 
for (i * 0; i < 16; i++) 

corrected_IR[i] - 2.2508 * ((double) IRRange[i] + 0.8602); 

for (i =0; i < 16; i++) 

corrected_sonar[i] = (double) SonarRange[i]; 
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/* Fuse the sonar and IR data, and store the final sensor reading into 
the global array fused_range[]. Infrared readings are not reliable 
beyond 14 inches. If the correlated value is smaller than 14, then 
normalize and fuse the infrared and sonar data. If it is more than 
14, then consider the sonar data. */ 
for (i =0; i < 16; i++) { 

if (IRRange[i] <= 14) 

{ 

norm[i] = corrected_sonar[i]*corrected_sonar[i]+ 
corrected_IR[i]*corrected_IR[i]; 
fused__range [i] = (corrected_sonar [i] *corrected_sonar [i] * 
corrected_IR[i] +corrected__IR[i] * 
corrected_IR[i]*corrected_sonar[i])/norm[i]; 
if (fused_range[i] <= 5.0) 
fused_range[i] = 0.0; 

} 

else 

fused_range [i] = corrected_sonar[i]; 

} 

/* The robot configuration parameters (x, y, steering angle, and 
turret angle) are stored in State [34], State[35], State[36], and 
State[37]. */ 

for (i = 0; i < 4; i++) 

robot_config[i] = State[34+i]; 

/*Check for bumper hit.If a bumper is activated, the corresponding bit 
in State[33] will be turned on. Since we don't care which bumper is 
hit, we thus only need to check if State[33] is greater than zero.*/ 

if (State[33] > 0) 

{ 

BumperHit = 1; 
tk ( "Ouch. ff ) ; 

printf("Bumper hit!\n”); 

} 


/* Find out the sensor ID number which detects the closest robot */ 

minreturn = 0; 

for (i = 1 ; i < 16 ; i++) { 

if (fused_range [i] < fused_range[minreturn]) 
minreturn = i ; 

} 

/* The distance to the closest robot */ 
mindist = fused_range[minreturn]; 

/* Find out the sensor ID number which detects the furthest robot */ 

maxreturn = minreturn ; 

for (i= 0 ;i< 16 ; i++) { 

if((fused_range[i]>=fused_range[maxreturn])&&(fused_range[i]<255.0)) 
maxreturn = i; 

} 


/* The distance to the furthest robot */ 
maxdist = fused_range[maxreturn]; 
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/* ID numbers of sensors that are next to the one which sees the 
closest robot */ 
min_above = minreturn+1 ; 
if ( min_above==16) 
min_above=0 ; 

min_below = minreturn-1 ; 
if ( min_below==-l) 
min__below==15; 

/* Check if the neighbor sensors see the same robot. If so, set the 
corresponding flag */ 
samel = 0 ; 
same2 = 0 ; 

if ( abs(fused_range[minreturn] - fused_range[min above]) <=4 ) 
samel = 1 ; “ 

if ( abs (fused__range [minreturn] - fused_range[min below]) <=4 ) 
same2 = 1 ; 

/*Find out the sensor ID number which sees the second closest robot*/ 
minreturn2 = maxreturn ; 
for (i = 0 ; i < 16 ; i++) { 

if(i!=minreturn) 

if (fused_range[i] < fused_range[minreturn2]) { 
if( (samel==l) && (i==min_above) ) 

printf("min_above == minreturn \n") ; 
else if( (same2==l) && (i==min_below) ) 
printf("min_below == minreturn \n") ; 

else 

minreturn2 = i ; 

} 

} 

/* The distance of the second closest robot */ 
mindist2 = fused_range[minreturn2] ; 


/* Sign function. It returns 1 if x is positive, and returns -1 
otherwise */ 
int sign(int x) 

{ 

return x>0?l:-l; 

} 

/**★**★*★* ★********************* / / 

/ ^ This function computes the attractive force as to the destination 
point in Robot Coordinate system and the repulsive force as to the 
obstacles in robot coordinate system. Finally it calculates the x and y 

components of the total forces on the robot again in robot coordinate 
system. */ 

void calculate () { 

double rho__0 = 15.0; /* Cut-off distance of the repulsive force */ 

double scale — 8.0 ; /* Scaling factor for attractive force */ 

double eta = 35000.0 ; /* Scaling factor for repulsive force */ 
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double rho_att = 15.0 ; /* Saturation distance for attractive 

force to switch between parabolic-well and 
conic-well */ 

double rho_float ; 

double range = rho__0 + (2*8.81) ; 

int i ; 

double xxl,xx2,yyl/yy2, xxb, yyb ; 
double length, distance ,minjnax; 
double xgoal, ygoal, xM, yM ; 
double teta, alpha, Csquare ; 
int signal = 0 ; 

/* The coordinates of the closest robot */ 

xxl = (double)(mindist+17.62)*cos((double)minreturn*0.39) ; 
yyl = (double)(mindist+17.62)*sin((double)minreturn*0.39) ; 

/* The coordinates of the second closest robot */ 

xxb = (double)(mindist2+17.62)*cos((double)minreturn2*0.39) ; 

yyb = (double)(mindist2+17.62)*sin((double)minreturn2*0.39) ; 

/* The coordinates of the furthest robot */ 

xx2 = (double)(maxdist+17.62)*cos((double)maxreturn*0.39) ; 

yy2 = (double)(maxdist+17.62)*sin((double)maxreturn*0.39) ; 

/* The distance between the furthest and the closest robots */ 
minjnax = hypot((yy2-yyl) , (xx2-xxl)) ; 

/* Check to see if there is only one contact around -Two consecutive 
sensors can return two different range values, although they both see 
the same robot- If there is only one robot detected, rotate around it. 
*/ 

if ((abs(minreturn-maxreturn)==1) || (abs(minreturn-maxreturn)==15) || 

(minreturn==maxreturn) ){ 

if ( abs(mindist-maxdist)<=4 ) { 

Csquare = (mindist*mindist) + (2*radius*2*radius) - 
(2*mindist*2*radius*cos(60.0*PI/180.0)); 


} 


alpha= acos( (double) ( (mindist*mindist) + Csquare - 

(2*radius*2*radius) ) / (2.0*mindist*sqrt(Csquare)) ); 

xgoal= sqrt(Csquare)*cos((double)(minreturn*0.39-alpha)) ; 

ygoal= sqrt(Csquare)*sin((double)(minreturn*0.39-alpha)) ; 
printf("There is only one contact around \n"); 

if ((ygoal==0)&&(xgoal==0)) 
teta = 0.0 ; 
else if (xgoal==0) 

teta = (90.0*PI/180.0) ; 

else 

teta = atan2(fabs(ygoal),fabs(xgoal)); 
signal = 1 ; 
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/* If there are more than one robot around */ 
if (!signal) { 

/* Compute the coordinates of the mid-point of the triangle that 
consists of the closest, second closest and the furthest robot at 
the corners. If there are only two robots around, then consider only 
the closest and furthest robot 1 s coordinates. */ 

if((minreturn2==maxreturn) || (((abs(minreturn2-maxreturn)==1) || 

(abs(minreturn2-maxreturn)==15)) && (abs(mindist2-maxdist)==2))){ 
xM=(xxl+xx2)/2.0 ; 
yM=(yy1+yy2)/2.0 ; 

} 

else { 

xM = (xxl+xx2+xxb)/3.0 ; 
yM = (yyl+yy2+yyb)/3.0 ; 

} 

/* The distance from robot's current position to the middle point 
between Rc, Rc2 and Rf */ 
length = hypot(xM,yM) ; 

/ Compute the coordinates of the point which is r distance away from 
the mid-point, where r is the desired radius of a circled to be 
formed. */ 

if ((yM==0)&&(xM==0)) 
teta = 0.0 ; 
else if (xM==0) 

teta = (90.0*PI/180.0) ; 

else 

teta - atan2(fabs(yM),fabs(xM)); 

xgoal = sign((int)xM) * (length-radius) * cos(teta) ; 

^ ygoal = sign((int)yM) * (length-radius) * sin(teta) ; 

distance — hypot(xgoal,ygoal); /^Distance to the goal configuration*/ 

/* Compute the attractive force in the robot coordinate system */ 
if (distance <=.rho_att) { 

F_att[0] = scale*xgoal ; 

F_att[l] = scale*ygoal ; 

} 

else { 

F_att[0] = scale*rho_att*(xgoal/distance) ; 

^ F_att[1] = scale*rho_att*(ygoal/distance) ; 

/* Compute the repulsive force in the robot coordinates */ 

F_rep[0] = 0.0; 

F_rep[1] = 0.0; 

for (i = 0; i <= 15; i++){ 

r ^°_float = (double) (fused_range[i]); 

if (rho_float < rho_0){ 

F_rep[0] += -eta*(1.0/rho_float - 1.0/rho_0)*cos((double)(i) * 

0.392699)/(rho float); 




} 


F_rep[l] += -eta*(1.0/rho_float - 1.0/rho_0)*sin((double)(i) * 
0.392699)/(rho_float); 


} 

/* compute the total force in the robot coordinates */ 
F_tol[0] = F_att[0] + F_rep[0]; 

F_tol[l] = F_att[1] + F_rep[1]; 
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APPENDIX C. SOURCE CODE FOR MOVING IN FORMATION ALGORITHM 


/* This is the source code of the moving in formation algorithm 

presented in chapter V. The ID number of the robot must be passed to 

the program as the first command line argument. The initial formation 
should be formed manually. The x and y coordinates of the goal point 
must be passed to each robot as the second and third command line 

arguments respectively. The same goal configuration must be passed to 

all robots. One copy of this program must be run for each robot with a 
different ID number. Nserver allows to simulate up to six robots, hence 
the robot ID number should be between 1 and 6. Only three robots are 
used in this simulation, as the Nserver does not show 100% reliability 
in the implementation of bulletin board communication scheme with more 
than three robots. */ 

#include "Nclient.h" 

#include <stdio.h> 

#include <stdlib.h> 

♦include <math.h> 

♦define PI 3.1415926 
♦define TRUE 1 
♦define FALSE 0 

/*** Function Prototypes ***/ 

void GetSensorData(void); 
void Move__Leader (void) ; 
void Move_Follower(void); 
int sign(int); 

/*** Global Variables ***/ 

double fused_range[16]; /* fused range data */ 
int BumperHit = 0; /* boolean value */ 

long robot_config [4]; /* the current robot configuration (x, y, 

steering angle, turret angle) x and y are in 
tenth of inches, and angles are in tenth of 
degrees */ 

long old_rob_config[2] ; 
long x_coord[7], y__coord[7] ; 
double xgoal_world, ygoal_world, m, d ; 
double Dold_goal[2] ; 

int Robot_ID, leader_ID, signal, stuck, count, count2, first ; 
int left, minreturn ; 
int robots[6] ; 

double old_dist ; 

/*** Main Program ***/ 

main (unsigned int argc, char* argv[]) 

{ 

int j,i,leader,svel ; 
int order[16]; 
double dist_array[7] ; 

double x_leader_rob, y_leader_rob, xgoal_rob, ygoal_rob, alpha ; 
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double xg__lw, yg_lw, xg_2w, yg_2w ; 
double gain^svel = 150.0 ; 

/*Store the x-y coordinates of the goal point into global variables*/ 
Robot_ID = atoi(argv[1]); 
xgoal_world = (double) atoi(argv[2]); 
ygoal_world = (double) atoi(argv[3]); 

/* Check the command line arguments. There should be three command 
line argument */ 
if (argc!=2) { 

printf("please enter one parameters besides the command\n"); 
exit(); 

} 

/* Check the robot ID, It should be between 1 and 6 */ 
if ( (Robot_ID<l) | | ( Robot__ID>6) ) { 

printf("Robot ID must be between 1 and 6 "); 
exit (); 

} 

/* Connect to Nserver.*/ 

SERV_TCP_PORT=7 771 ; 

strcpy (ROBOT JHACHINEJNAME, "nomad") ; 

connect_robot(Robot_ID); 

/* Initialize Smask and send to robot. Smask is a large array that 
controls which data the robot returns back to the server. This 
function tells the robot to give us everything. */ 
init_mask(); 

/* Configure timeout (given in seconds). This is how long the robot 
will keep moving if it becomes disconnected. */ 
conf_tm(5); 

/* Sonar setup: configure the order in which individual sonar units 
fire. In this case, fire all units in counter-clockwise order (units 
are numbered counter-clockwise starting with the front sonar as zero) 
The conf_sn() function takes an integer and an array of at most 16 
integers. If less than 16 units are to be used, the list must be 
terminated by a element of value -1. See the IR setup below for an 
example of this. ‘The single integer value passed controls the time 
delay between units in multiples of four milliseconds. */ 
for (j = 0; j < 16; j++) 
order[j] = j; 

conf_sn(1,order); 

/* Configure the Infrared Sensors */ 
for (j = 0; j < 16; j++) 
order[j] = j; 

conf_ir(1,order); 

GetSensorData(); 

/* Find the distance of each robot from the goal point, and put them 
in dist_array[] */ 
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for(i=0 ; i<signal ; i++) 

dist_array[i] = hypot((xgoal_world - (double) x_coord[robots[i] ]) , 

(ygoal_world - (double)y_coord[robots[i]])) ; 

/* Find the closest robot to the goal point. If two or more robot 1 s 
are exactly the same distance away from the goal, then choose the one 
with smallest ID number. This will be the leader of formation. */ 

leader = 0 ; 

for(i=l ; i<signal ; i++) { 

if(dist_array [i] < dist_array[leader]) 
leader - i ; 

} 

leader_ID = robots[leader] ; 
svel = 10 ; 

/* Turn robot’s face towards the goal point. The robot faces towards 
the goal point if the rotational velocity is less than or equal to 2 
degrees/sec. */ 

while(abs(svel) > 2){ 

GetSensorData() ; 

alpha = (double) robot_config [2] *PI/1800.0 ; 

/^Coordinates of the goal point in robot’s local coordinate system*/ 
xgoal_rob = xgoal_world*cos(alpha) + ygoal_world*sin(alpha) - 
(double) robot_config[0]*cos(alpha)- 
(double) robot__config [1] *sin (alpha) ; 

ygoal__rob = -xgoal_world*sin(alpha) + ygoal_world* cos (alpha) + 
(double)robot_config[0]*sin(alpha)- 
(double)robot_config[1]*cos(alpha) ; 

svel = (int)(gain_svel*sin(atan2(ygoal_rob,xgoal_rob))) ; 

if(abs(svel)>450) 

svel=450*sign(svel) ; 
vm(0,svel, svel) ; 

} 


stuck = 0 ; 
count = 0 ; 
first = 0 ; 
left = 1 ; 

old_rob_config[0] = 0 ; 
old_rob__config [1] = 0 ; 

if (leader_ID=Robot_ID) { /* If this robot is the leader */ 

st () ; 

sleep(2) ; /* Wait for two seconds for the other robots */ 

/* Execute the Move_Leader() function as long as there is no 

collision */ 

while(!BumperHit) { 

Mov^Leader () ; 

} 
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else { 


/* If this robot is not the leader */ 


/* Compute the coordinates of the leader in robot's local 
coordinate system */ 

x_leader_rob = x_coord[leader_ID]*cos(alpha) + 
y_coord[leader_ID]*sin(alpha) - 
(double)robot_config[0]*cos(alpha) - 
(double)robot_config[1]*sin(alpha); 

y_leader_rob = -x_coord[leader_ID]*sin(alpha) + 
y_coord[leader_ID]*cos(alpha)+ 

(double)robot_config[0]*sin(alpha)- 
(double)robot_config[1]*cos(alpha); 

/* Determine which side is the leader located : left or right. */ 
if((sign(x_leader_rob)*sign(y_leader_rob))<0) 

= ~1 ! /* Leader is on the right side as to the 

direction of motion in formation*/ 

/* Slope of the line that passes through the leader and the robot 
itself in world coordinate system. */ 
m = (double)(y_coord[leader_ID] - 

robot_config[1])/(double) (x_coord[leader_ID]-robot_config[0] ) ; 

/* Distance from the leader */ 

d = hypot((double) (y_coord[leader_ID] - robot_config[1]) , 

(double)(x_coord[leader_ID] - robot_config[0])); 

/* The coordinates of the goal point which is "d" distance away 

from the leader's goal point on the line that has a slope of "m" - 

in world coordinate system */ 

xg_lw = xgoal_world + d/sqrt(1.0+m*m) ; 

yg_lw = ygoal_world + m*d/sqrt(1.0+m*m) ; 

xg_2w = xgoal_world - d/sqrt(1.0+m*m) ; 
yg_2w = ygoal_world - m*d/sqrt(1.0+m*m) ; 

/* Chose the closer one between these two points */ 

if ( hypot((xg_lw-robot_config[0]),(yg_lw-robot_config[1])) < 

^ hypot((xg_2w-robot_config[0]),(yg_2w-robot_config[1])) ) 

xgoal_world = xg_lw ; 
ygoal_world = yg_lw ; 

} 

else 

{ 

xgoal_world = xg_2w ; 
ygoal_world' = yg_2w ; 

} 


/* Execute the Move_Follower() function as long as there is no 
collision */ 

while(!BumperHit) { 

Move_Follower(); 

} 
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/* Disconnect. */ 
vm (0, 0, 0) ; 

disconnect robot(Robot ID); 


/******************************/ 

/* This function determines the movements of the leader. The leader 
simply moves towards the given goal location independently, while 
avoiding any collision based on the potential field method */ 

void Move_Leader(void) 

{ 

int i; 

int tvel, svel; 

double F_att[2], F_rep[2], F_tol[2]; 

double gainjtvel = 0.09; /* Translational velocity gain */ 

double gain_svel = 150.0; /* Rotational velocity gain */ 

/* Compute the attractive force as to the destination point and the 
repulsive force as to the obstacles in Robot Coordinate system . 
Finally calculate the x and y components of the total forces on the 
robot, again in robot coordinate system. */ 

double rho_0 = 15.0; /* Cut-off distance of the repulsive force */ 

double scale = 7.0 ; /* Scaling factor for attractive force */ 

double eta = 35000.0 ; /* Scaling factor for repulsive force */ 

double rho_att = 15.0; /* Saturation distance for attractive 

force to switch between parabolic-well and 
conic-well */ 

double rho_float ; 

double D_goal [ 2 ] , X__compl, X_comp2, X_comp3 ; 

double Y_compl, Y_comp2, Y_comp3 ; 

int dirdiff,panic ; 

double distance,alpha ; 

double xgoal, ygoal ; 

GetSensorData() ; 

/* Set the panic flag if there is any contact within 8 inches on the 
direction of motion */ 

panic = FALSE; 

for (i = 12; i <= 15; i++) 

if (fused_range[i] < 8 ) panic = TRUE; 
for (i = 0; i <= 4; i++) 

if (fused_xange[i] < 8 ) panic = TRUE; 

if(!stuck) { /* If you are not stuck */ 

/* If the robot moves less than 2 inches two successive times 
although it is away from its goal location, then set the stuck 
flag */ 

if (hypot((double)(robot_config[0]old_rob_config[0]), 

(double)(robot_config[1]-old_rob_config[1]))<= 2.0){ 

count++ ; 
if(count==2){ 


if(old_dist >5.0) { 
stuck = 1 ; 
first = 1 ; 

} 

count = 0 ; 

} 

} 

/* Store the robot*s position to remember in the next iteration */ 
old_rob_config[0] = robot__config [0] ; 
old_rob_config[l] = robot_config[1] ; 

alpha = (double)robot_config[2]*PI/1800.0 ; 

/* Coordinates of the goal point in robot’s local coordinate 
system */ 

xgoal = xgoal__world*cos(alpha) + ygoal_world*sin(alpha) - 
(double)robot_config[0]*cos(alpha)- 
(double)robot_config[1]*sin(alpha); 

ygoal = -xgoal__world*sin(alpha) + ygoal_world*cos(alpha) + 

(double)robot_config[0]*sin(alpha)- 
(double)robot_config[1]*cos(alpha) ; 

distance = hypot(xgoal,ygoal) ; /* Distance to the goal point */ 
old__dist = distance ; 

/* Compute the attractive force in the robot coordinate system */ 
if (distance <= rho_att) { 

F_att[0] = scale*xgoal ; 

F_att[l] = scale*ygoal ; 

} 

else { 

F_att[0] = scale*rho_att*(xgoal/distance) ; 

^ F_att [ 1 ] = scale*rho__att* (ygoal/distance) ; 

/* Compute the repulsive force in the robot coordinate system */ 
F_rep[0] = 0.0; 

F_rep[1] = 0.0; 

for (i = 0; i <= 15; i++){ 

r ho_float = (double) (fused_range[i]); 
if (rho_float < rho__0){ 

F_ re P[0] += -eta*(1.0/rho_float -1.0/rho_0)*cos((double)(i)* 
0.392699)/(rho_float); 

F_rep[l] += -eta* (1.0/rho__float -1.0/rho_0) *sin ( (double) (i) * 
0.392699)/(rho float); 

} 

} 

/* compute the total force in the robot coordinate system */ 

F_tol[0] - F_att[0] + F_rep[0]; 

F_tol[l] = F_att[l] + F__rep[1]; 

/* set the translational velocity */ 
tvel = (int) (gain_tvel * F_tol[0]); 
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/* set the rotational velocity */ 
if ((F_tol[0]==0)&&(F_tol[1]==0)) 
svel = 0 ; 

else if ( F_tol[0]~0 ) 
svel = 450 ; 
else 

svel = (int) (gain_svel * sin(atan2(F_tol[1],F_tol[0]))); 
svel = svel * sign((int) (F_tol[0])) ; 

/* limit the translational and rotational velocities */ 
if (abs(tvel) > 240) 

tvel = 240 * sign(tvel); 

if (abs(svel) > 450) 

svel = 450 * sign(svel) ; 

/* Set the robot 1 s velocities. The first parameter is the robot’s 
translational velocity, in tenths of an inch per second. This 
velocity can be between -240 and 240. The second parameter is the 
steering velocity, and the third is the turret velocity. The units 
of the latter two are tenths of a degree per second, and can be 
between -450 and 450. The same value is given for these two so 
that the turret is always facing the direction of motion. */ 

printf("tvel=%d svel=%d \n",tvel,svel); 
vm(tvel,svel,svel); 

} 

else /* If you are stuck */ 

{ 

D_goal[0] = (double) (xgoal_world-robot__config[0]) ; /*x component */ 
D_goal[l] = (double)(ygoal_world-robot_config[1]); /*y component */ 

distance = hypot(D_goal[0],D__goal[1] ) ; 

/* If you entered this block for the first time, meaning that you 
have just realized that you were stuck, make a 90 degree turn to 
left as to the direction of motion, and leave the obstacle on your 
right */ 

if ( first ){ 
dirdiff=200; 
printf("first\n"); 

while(! (abs(dirdiff)<100) ) 

{ 

tvel=0; 

svel=dirdiff/5; 
vm(tvel,svel,svel) ; 

GetSensorData() ; 

alpha = ((double)robot_config[2])*PI/(1800.0); 
xgoal = cos (alpha) *D_goal [0] + sin (alpha) *D__goal [ 1] ; 
ygoal = -sin(alpha)*D_goal[0] + cos(alpha)*D_goal[1]; 
dirdiff = (int)(atan2(ygoal,xgoal)*1800.0/PI) + 1100 ; 

} 

first=0; /* Reset the first flag */ 
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} 


Dold_goal[0] = 0.0; 
Dold_goal[l] = 0.0; 


if (’panic) 
tvel = 10; 
else 

tvel = 0; 

zobot moves towards the final goal point more than 16 
inches for 5 successive times, it means, it turned the corner of 
the obstacle and started to move towards the direction of motion of 
the formation. In this case reset the stuck flag. */ 
if ( distance < (hypot(Dold_goal[0],Dold_goal[1])-16.0)) { 
count2++; 
if (count2==5) { 
stuck = 0 ; 
count2 = 0 ; 

} 

} 

Dold_goal[0]=D_goal[0] ; 

Dold_goal[1]=D_goal[1]; 

/* Wat ch the empty space on the direction of motion while following 
the edge of the obstacle, and adjust the rotational velocity in 
order to follow the edge and turn around the corner of the 
obstacle. */ 

X_compl = (double)fused_range[15] * cos(2.0*PI*15.0/16.0); 

X_comp2 = (double)fused^range[0] * cos(2.0*PI*0.0 /16.0); 

X_comp3 - (double)fused_range[1] * cos(2.0*PI*1.0 /16.0); 

if ((X_compl<15.0)||(X_comp2<15.0)||(X_comp3<15.0)){ 
svel = 65*sign(left); 
tvel = 5; 
stuck = 0 ; 
count2 = 0 ; 

} 

else 

svel=0; 

/* Also watch out the distance from the obstacle while following 
the edges. Try to keep the same distance from the obstacle by 
adjusting the rotational velocity */ 

Y_compl = (double)fused^range[14] * sin(2.0*PI*2.0/16.0); 

Y _comp2 = (double)fused_range[13] * sin(2.0*PI*3.0/16.0); 

Y _ com P 3 = (double)fused_range[12] * sin(2.0*PI*4.0/16.0); 

svel = svel - sign(left)*(int)(Y_compl + Y_comp2 + Y_comp3 - 200.0); 

if (abs(svel)<100) 
svel=svel; 

else 

svel=svel*20/36; 

if(stuck==0) { 
tvel = 0 ; 
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svel 


- 0 ; 

} 

if(panic==l) 

svel = sign (left)*50 ; 

vm(tvel f svel,svel); 

} /* End else */ 

} 

/**************************/ 

/* This function determines the movements of the follower robots. Each 
follower robot determines a new goal location for itself at each 
iteration based on the current position of the leader, and moves to this 
point while avoiding any collision, based on the potential field method. 
*/ 

void Move_Follower(void) 

{ 

int I, tvel, svel; 

double F_att[2], F_rep[2], F_tol[2]; 

double gain_tvel = 0.1; /* Translational velocity gain */ 

double gain_svel = 150.0; /* Rotational velocity gain */ 

/* Compute the attractive force as to the destination point and the 
repulsive force as to the obstacles in Robot Coordinate system. 

Finally calculate the x and y components of the total forces on the 
robot again in robot coordinate system. */ 

double rho_0 = 15.0; /* Cut-off distance of the repulsive force */ 

double scale = 7.0 ; /* Scaling factor for attractive force */ 

double eta = 35000.0 ; /* Scaling factor for repulsive force */ 

double rho_att = 15.0 ; /* Saturation distance for attractive force 

to switch between parabolic-well and 
conic-well */ 

double rho_float ; 

double D_goal[2],X_compl,X_comp2,X_comp3 ; 

double Y_compl,Y_comp2,Y_comp3 ; 

int dirdiff,sonl,son2,son3,panic,panicl ; 

int see_leader = 0 ; 

double distance ; 

double xgoal, ygoal; 

double xg_l,yg_l, xg_2,yg_2 ; 

double xg_lw,yg_lw, xg_2w,yg_2w ; 

double teta ; 

GetSensorData() ; 

/* Set the panic flag if there is any contact within 8 inches on the 
direction of motion */ 

panic = FALSE; 
panicl = FALSE ; 

for (i = 12; i <- 15; i++){ 

if (fused_range[i] < 8 ) 
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panic = TRUE; 

else if (fused_range[i] < 20) 
panicl = TRUE ; 

} 

for (i = 0; i <= 4; i++) { 

if (fused_range[i] < 8 ) 
panic = TRUE ; 

else if (fused_range[i] < 20) 
panicl = TRUE ; 

} 


if(!stuck) { /* If you are not stuck */ 

/* If the robot moves less than 3 inches two successive times 
although it is away from its goal location, then set the stuck 
flag */ 

if (hypot((double)(robot_config[0]-old rob configfO]), 

(double)(robot_config[1]-old~rob~config[1]))<=3.0 ){ 
count++ ; 
if(count==2){ 

if(old_dist >8.0) { 

if(fused_range[minreturn]<(rho_0+l.0)){ 
stuck = 1 ; 
first = 1 ; 

} 

} 

count - 0 ; 

} 

} 

old_rob_config[0] = robot_config[0] ; 
old_rob_config[1] = robot_config[1] ; 

teta = (double)robot_config[2]*PI/1800.0 ; 

/* Check to see if you can get the position information of the 
leader. Depending on the distance and the location of the leader, 
sometimes it is not possible to reach the position information of 
the leader, because of the implementation of get_rpx() function 
in Nserver */ 


for(i=l ; i<signal ; i++) { 

if(robots[i]==leader_ID) 
see_leader=l; 


} 


if(see_leader) { /* If you can get the position information of 

the leader */ 

/* The coordinates of the point which is "d" distance away from 
the leader on the line that has a slope of "m" - in world 
coordinate system. */ 

xg_lw = x_coord[leader_ID] + d/sqrt(1.0+m*m) ; 
yg_lw = y_coord[leader_ID] + m*d/sqrt{1.0+m*m) ; 
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xg_2w = x_coord[leader_ID] - d/sqrt(1.0+m*m) ; 

yg__2w = y__coord [leader_ID] - m*d/sqrt {1.0+m*m) ; 

/* Convert the coordinates of these points into robot 1 s local 
coordinate system. */ 

xg_l = xg_lw*cos (teta) + yg__lw^sin (teta)- 
(double) robot_config [0] *cos (teta) - 
(double)robot_config[1]*sin(teta) ; 

yg_1 = -xg_lw*sin(teta) + yg_lw*cos(teta)+ 

(double)robot_config[0]*sin(teta)- 
(double)robot_config[1]*cos(teta) ; 

xg_2 = xg_2w*cos (teta) + yg__2w*sin (teta) - 
(double)robot_config[0]*cos(teta)- 
(double)robot_config[1]*sin(teta) ; 

yg__2 = -xg_2w*sin (teta) + yg_2w*cos(teta)+ 

(double)robot_config[0]*sin(teta)- 
(double)robot_config[l]*cos(teta) ; 

/* Chose the closer one among these two points as the goal 
configuration */ 

if ( hypot(xg_l,yg_l) < hypot(xg_2,yg_2)' ) { 

xgoal = xg_l ; 
ygoal = yg_l / 

} 

else { 

xgoal = xg_2 ; 
ygoal = yg_2 ; 

} 

} 

else { /* If you cannot get the position information of the 

leader, move towards the final goal location faster 
than the formation speed. */ 
if(!panicl) 

gain_tvel = 0.15 ; 

gain_svel = 250 ; 

/* Coordinates of the final goal point in robot’s local 
coordinate system */ 

xgoal = xgoal_world*cos(teta) + ygoal_world*sin(teta) - 
(double)robot_config[0]*cos(teta)- 
. (double)robot_config[1]*sin(teta) ; 

ygoal = -xgoal_world*sin(teta) + ygoal_world*cos(teta) + 
(double)robot_config[0]*sin(teta)- 
(double)robot_config[1] + cos(teta) ; 

} 

distance = hypot(xgoal,ygoal) ; /* Distance to the goal point */ 

old__dist = distance ; 

/* Compute the attractive force in the robot coordinate system */ 
if (distance <= rho_att) { 
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F_att[0] = scale*xgoal ; 
F_att[l] = scale*ygoal ; 


} 

else { 

F_att[0] = scale*rho_att*(xgoal/distance) ; 

F_att[l] = scale*rho att*(ygoal/distance) ; 

} 

/^Increase the translational velcity gain to speed up the robot*/ 
if (see_leader) 

if (distance > (3*rho__0)) 
if (ipanicl) 

gain_tvel =0.15 ; 

/* Compute the repulsive force in the robot coordinates */ 

F_rep[0] = 0.0; 

F_rep[1] = 0.0; 

for (i = 0; i <= 15; i++) { 

= (double) (fused_range[i]); 
if (rho__float < rho_0) { 

F_rep [0] += -eta*(1.0/rho_float -1.0/rho_0)*cos((double) (i)* 
0.392699)/(rho_float); 

F__rep [ 1 ] += -eta* (1.0/rho_float -1.0/rho_0) *sin ( (double) (i) * 
0.392699)/(rho float); 

} 

} 

/* compute the total force in the robot coordinates */ 

F_tol[0] = F_att[0] + F_rep[0]; 

F_tol[l] = F_att[1] + F_rep[1]; 

/* set the translational velocity */ 
tvel = (int) (gain_tvel * F___tol[0]); 

/* set the rotational velocity */ 
if ( (F_tol [0] ==0) && (F_tol [ 1 ] ==0-) ) 
svel = 0 ; 

else if ( F_tol[0]==0 ) 
svel = 450 ; 
else 

svel = (int) (gain_svel * sin(atan2(F_tol[1],F_tol[0]))); 
svel = svel * sign((int)(F_tol[0])); 

/* limit the translational and rotational velocities */ 
if (abs(tvel) > 240) 

tvel = 240 * sign(tvel); 

if (abs(svel) > 450) 

svel = 450 * sign(svel) ; 

/* Set the robot*s velocities. The first parameter is the robot*s 
translational velocity, in tenths of an inch per second. This 
velocity can be between -240 and 240. The second parameter is the 
steering velocity, and the third is the turret velocity. The units 
of the latter two are tenths of a degree per second, and can be 
between -450 and 450. The same value is given for these two so 
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that the turret is always facing the direction of motion. */ 
vm(tvel,svel,svel); 

} 

else { /* If you are stuck */ 

D_goal[0] = (double)(xgoal_world-robot_config[0]); /* x component */ 
D_goal[l] = (double) (ygoal_world-robot__conf ig [1] ) ; /* y component */ 

distance = hypot(D_goal[0],D_goal[1]) ; 

/* If you entered this block for the first time, meaning that you 
have just realized that you were stuck, make a 90 degree turn 
towards the main body of the formation */ 

if ( first ){ 

dirdiff=200; 

while(! (abs(dirdiff)<100) ) { 

tvel=0; 

svel=dirdiff/5; 
vm(tvel,svel,svel); 

GetSensorData() ; 


teta = ((double)robot_config[2])*PI/(1800-0); 
xgoal = cos (teta) *D__goal [0] + sin (teta) *D_goal [ 1 ] ; 
ygoal = -sin(teta)*D_goal[0] + cos (teta) *D_goal [ 1 ] ; 


dirdiff 


} 

first=0; 
Dold_goal[0] 
Dold_goal[1] 


(int)(atan2(ygoal,xgoal)*1800.0/PI) + 
sign(left)*1100 ; 


0 . 0 ; 

0 . 0 ; 


/* Set the translational velocity according to panic flag */ 
if (!panic) 
tvel =20; 
else 

tvel = 0; 

/* If the robot moves towards the final goal point more than 30 
inches for 4 successive times, it means, it turned the corner of the 
obstacle and started to move towards the direction of motion of the 
formation. In this case reset the stuck flag. */ 

if ( distance < (hypot(Dold_goal[0],Dold_goal[1])-30.0)) { 
count2++ ; 
if(count2==4) { 

stuck = 0; 
count2 = 0 ; 

} 


Dold_goal[0]=D_goal[0] ; 
Dold_goal[1]=D_goal[1 ] ; 
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/* w ^tch the empty space on the direction of motion while following 
the edge of the obstacle, and adjust the rotational velocity in 
order to follow the edge and turn around the corner of the obstacle. 
*/ 

X_compl = (double)fused_range[15] * cos(2.0*PI*15.0/16.0); 

X_comp2 = (double)fused_range[0] * cos(2.0*PI*0.0 /16.0); 

X_comp3 = (double)fused_range[1] * cos (2.0*PI*1.0 /16.0); 

if ( (X_compl<15.0) | | (X_comp2<15.0) | | (X__comp3<15.0) ) { 
svel = 65*sign(left); 
tvel = 5; 
stuck = 0 ; 
count2 = 0 ; 

} 

else 

svel=0; 

/* Also w atch out the distance from the obstacle while following the 
edges. Try to keep the same distance from the obstacle by adjusting 
the rotational velocity. Sensor numbers are determined as to which 
side is the obstacle. */ 

if (left==l) { 
sonl =14 ; 
son2 =13 ; 

son3 = 12 ; 

} 

else { 

sonl = 2 ; 
son2 = 3 ; 

son3 = 4 ; 

} 

Y _compl = (double)fused_range[sonl] * sin(2.0*PI*2.0/16.0) ; 

Y _ com p2 = (double)fused_range[son2] * sin(2.0*PI*3.0/16.0); 

Y_comp3 = (double)fused_range[son3] * sin(2.0*PI*4.0/16.0); 

if(left==l) 

svel = svel - (int)(Y_compl + Y_comp2 + Y comp3 - 90.0); 
else 

svel = svel + (int) (Y__compl + Y_comp2 + Y__comp3 - 100.0); 

if (abs(svel)<100) 
svel=svel; 

else 

svel=svel*20/36; 

if (stuck==0) { 

tvel = 0 ; 
svel = 0 ; 

} 


vm(tvel,svel,svel); 

} 

} 
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/* Read in sensor data and load into arrays. */ 
void GetSensorData () 

{ 

int i,index; 

double corrected_IR[16] ; /* Correlate infrared reading to distance */ 
long SonarRange[16]; /* Array of sonar readings (inches) */ 

long IRRange[16]; /* Array of infrared readings (no units)*/ 

double corrected_sonar[16] ; /* Array to store the corrected 

sonar readings */ 

double norm[16] ; 
long rob__pos[16] ; 

/* Read all sensors and load data into State array. */ 
gs () ; 

/* Read State array data and put readings into individual arrays. */ 
for (i = 0; i < 16; i++) 

{ 

/* Sonar ranges are given in inches, and can be between 6 and 255, 
inclusive. */ 

SonarRange[i] = State[17+i]; 

/* IR readings are between 0 and 15, inclusive. This value is 
inversely proportional to the light reflected by the detected 
object, and is thus proportional to the distance of the object. 

Due to the many environmental variables effecting the reflectance 
of infrared light, distances cannot be accurately ascribed to the 
IR readings. */ 

IRRangefi] = State[1+i]; 

} 

/* To correlate the infrared reading to physical distance. The numbers 
are obtained by least square linear regression of measurement data. */ 

for (i = 0; i < 16; i++) 

corrected__IR[i] = 2.2508 * ((double) IRRange[i] + 0.8602); 

for (i - 0; i < 16; i++) 

corrected_sonar[i] = (double) SonarRange[i]; 

/* Fuse the sonar and IR data, and store the final sensor reading into 
the global array fused_range[]. Infrared readings are not reliable 
beyond 14 inches. If the correlated value is smaller than 14, then 
normalize and fuse the infrared and sonar data. If it is more than 
14, then consider the sonar data. */ 

for (i « 0; i < 16; i++) 

{ 

if (IRRange[i] <= 14) 

{ 

norm[i] = corrected_sonar [i] *corrected__sonar [i] + 
corrected_IR[i] *corrected__IR[i] ; 

fused_range [i] = (corrected_sonar [i] *corrected__sonar [i] * 
corrected_IR[i] + corrected_IR[i]* 
corrected_IR[i] * corrected__sonar [i] ) /normfi] ; 


73 



if (fused_range[i] <= 5.0) 
fused__range [i] = 0.0; 

} 

else 

fused_range[i] = corrected_sonar[i]; 

/* Find out the sensor ID number which detects the closest robot */ 

minreturn = 0; 

for (i = 1 ; i < 16 ; i++) { 

if (fused_range [i] < fused__range [minreturn] ) 
minreturn = i ; 

} 

/* The robot configuration parameters (x, y , steering angle, and 
turret angle) are stored in State [34], State[35], State [36], and 
State[37]. */ 

for (i = 0; i < 4; i++) 

robot_config[i] = State[34+i]; 

/* Check for bumper hit. If a bumper is activated, the corresponding 
bit in State[33] will be turned on. Since we don’t care which bumper 
is hit, we thus only need to check if State[33] is greater than zero. 
*/ 

if (State[33] > 0) 

{ 

BumperHit =1; 
t k ( "Ouch. ") ; 

print f ( "Bumper hit!\n ,T ); 


/* Get the position information of the robots around in robot's local 
coordinate system */ 

get_rpx(rob_pos) ; 

/* Put the x - y coordinates of each robot in array, into the cell 
that corresponds to each robot's ID number */ 

x_coord[Robot_ID] = robot_config[0] ; 
y_coord[Robot_ID] = robot_config[1] ; 

for(i=l ; i<(3*rob_pos[0]) ; i+=3 ) { 

x_coord[rob_pos[i]] = rob_pos[i+l] + robot_config[0] ; 

^ y_coord[rob_pos[i]] = rob_pos[i+2] + robot_config[1] ; 

robots[0]=Robot_ID ; 
index = 1 ; 

for(i=l ; i<=rob_pos[0] ; i++){ 
robots[i]=rob_pos[index] ; 
index+=3 ; 

} 

signal = i ; 
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/* Sign function. It returns 1 if x is positive, and returns -1 
otherwise */ 
int sign(int x) 

{ 

return x>0?l:-l; 

} 
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